From 659fee9a261c99e9e14975858419b00399600757 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 28 May 2024 10:56:27 -0700 Subject: [PATCH 001/126] CI: added jazzy build on ros2 workflow --- .github/workflows/ros2.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index b810295b..86f60afa 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -21,12 +21,14 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - ros_distro: [humble, iron] + ros_distro: [humble, iron, jazzy] include: - ros_distro: 'humble' os: ubuntu-22.04 - ros_distro: 'iron' os: ubuntu-22.04 + - ros_distro: 'jazzy' + os: ubuntu-24.04 steps: - uses: ros-tooling/setup-ros@v0.6 From 20d5bb58d4422b281a0abd389d4c7c12151db62c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 28 May 2024 10:59:07 -0700 Subject: [PATCH 002/126] CI: setup-ros v0.7 --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 86f60afa..9e1c425d 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -31,7 +31,7 @@ jobs: os: ubuntu-24.04 steps: - - uses: ros-tooling/setup-ros@v0.6 + - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} From f1b80d256996661c119cfcc7895c031430ab8100 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 28 May 2024 11:14:13 -0700 Subject: [PATCH 003/126] CI-ros2: using docker approach --- .github/workflows/ros2.yml | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 9e1c425d..1e30cff5 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -17,19 +17,20 @@ jobs: # 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 on ros2 ${{ matrix.ros_distro }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} + name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.docker_image }} + runs-on: ubuntu-latest strategy: matrix: ros_distro: [humble, iron, jazzy] include: - ros_distro: 'humble' - os: ubuntu-22.04 + docker_image: ubuntu:jammy - ros_distro: 'iron' - os: ubuntu-22.04 + docker_image: ubuntu:jammy - ros_distro: 'jazzy' - os: ubuntu-24.04 - + docker_image: ubuntu:noble + container: + image: ${{ matrix.docker_image }} steps: - uses: ros-tooling/setup-ros@v0.7 with: From fe02ec394877c461fd6cb98cbe572e32651499fc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 14:58:44 -0700 Subject: [PATCH 004/126] CI: added shell bash --- .github/workflows/ros2.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 1e30cff5..0b954598 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -37,6 +37,7 @@ jobs: required-ros-distributions: ${{ matrix.ros_distro }} - name: Setup ros2 workspace + shell: bash run: | source /opt/ros/${{ matrix.ros_distro }}/setup.bash mkdir -p ${{github.workspace}}/ros2_ws/src @@ -53,6 +54,7 @@ jobs: path: 'ros2_ws/src/rtabmap_ros' - name: colcon build + shell: bash run: | source /opt/ros/${{ matrix.ros_distro }}/setup.bash cd ${{github.workspace}}/ros2_ws From 95875c872e56dc6176582706eee5b4874dcd0e52 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 15:09:38 -0700 Subject: [PATCH 005/126] CI: update shell command --- .github/workflows/ros2.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 0b954598..b4b79738 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -37,7 +37,7 @@ jobs: required-ros-distributions: ${{ matrix.ros_distro }} - name: Setup ros2 workspace - shell: bash + shell: /usr/bin/bash -e {0} run: | source /opt/ros/${{ matrix.ros_distro }}/setup.bash mkdir -p ${{github.workspace}}/ros2_ws/src @@ -54,7 +54,7 @@ jobs: path: 'ros2_ws/src/rtabmap_ros' - name: colcon build - shell: bash + shell: /usr/bin/bash -e {0} run: | source /opt/ros/${{ matrix.ros_distro }}/setup.bash cd ${{github.workspace}}/ros2_ws From 0704e91d445af8fca27829cd2a6728c11e33f477 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 15:24:39 -0700 Subject: [PATCH 006/126] ci: using action-ros-ci --- .github/workflows/ros2.yml | 30 +++++------------------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index b4b79738..884088d1 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -19,6 +19,7 @@ jobs: # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.docker_image }} runs-on: ubuntu-latest + fail-fast: false strategy: matrix: ros_distro: [humble, iron, jazzy] @@ -35,29 +36,8 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - - - name: Setup ros2 workspace - shell: /usr/bin/bash -e {0} - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - mkdir -p ${{github.workspace}}/ros2_ws/src - cd ${{github.workspace}}/ros2_ws - colcon build - - - uses: actions/checkout@v2 + + - uses: ros-tooling/action-ros-ci@v0.3 with: - repository: 'introlab/rtabmap' - path: 'ros2_ws/src/rtabmap' - - - uses: actions/checkout@v2 - with: - path: 'ros2_ws/src/rtabmap_ros' - - - name: colcon build - shell: /usr/bin/bash -e {0} - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - cd ${{github.workspace}}/ros2_ws - rosdep update - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool - colcon build --event-handlers console_direct+ + package-name: rtabmap_ros + target-ros2-distro: ${{ matrix.ros_distro }} From 2a511b27874dc38e4e7f973ada6444dd33762e7f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 15:27:23 -0700 Subject: [PATCH 007/126] ci: fixed fail-fast --- .github/workflows/ros2.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 884088d1..f0d01f87 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -19,7 +19,6 @@ jobs: # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.docker_image }} runs-on: ubuntu-latest - fail-fast: false strategy: matrix: ros_distro: [humble, iron, jazzy] @@ -30,13 +29,13 @@ jobs: docker_image: ubuntu:jammy - ros_distro: 'jazzy' docker_image: ubuntu:noble + fail-fast: false container: image: ${{ matrix.docker_image }} steps: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: rtabmap_ros From 9b6e9d6bbe3b2743aa2459d72e5f5c348a8d31f0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 15:45:36 -0700 Subject: [PATCH 008/126] ci: installing upstream rtabmap master from source --- .github/workflows/ros2.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index f0d01f87..fc2e36e6 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -36,7 +36,10 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} + - run: | + echo -e "- git:\n local-name: rtabmap\n uri: https://github.com/introlab/rtabmap.git\n version: master" > /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: rtabmap_ros target-ros2-distro: ${{ matrix.ros_distro }} + vcs-repo-file-url: /tmp/deps.repos From 08d541319c0f9e4fb0c5df8b422a2822488bbec8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 15:59:38 -0700 Subject: [PATCH 009/126] ci: missing action checkout --- .github/workflows/ros2.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index fc2e36e6..079777a1 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -33,11 +33,12 @@ jobs: container: image: ${{ matrix.docker_image }} steps: + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - run: | - echo -e "- git:\n local-name: rtabmap\n uri: https://github.com/introlab/rtabmap.git\n version: master" > /tmp/deps.repos + echo -e "- git:\n local-name: rtabmap\n uri: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: rtabmap_ros From 3b00ff2c0c94696574095c062735791b2cbdc0ec Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 16:20:34 -0700 Subject: [PATCH 010/126] ci: fixed checkout upstream --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 079777a1..a96279a4 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,7 +38,7 @@ jobs: with: required-ros-distributions: ${{ matrix.ros_distro }} - run: | - echo -e "- git:\n local-name: rtabmap\n uri: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos + echo -e "repositories:\n rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: rtabmap_ros From 2127a1711e2e212372e5d6966070e8064a2a1083 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 16:27:47 -0700 Subject: [PATCH 011/126] ci: update --- .github/workflows/ros2.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index a96279a4..460bf530 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,7 +38,8 @@ jobs: with: required-ros-distributions: ${{ matrix.ros_distro }} - run: | - echo -e "repositories:\n rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos + echo -e "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos \ + cat /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: rtabmap_ros From ecf94e0e8d8d7ad81427ddef4b02c5c234c5d615 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 16:33:47 -0700 Subject: [PATCH 012/126] ci: update --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 460bf530..63c5f663 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,7 +38,7 @@ jobs: with: required-ros-distributions: ${{ matrix.ros_distro }} - run: | - echo -e "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos \ + echo -e "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\ cat /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: From db99ea0eb559e0edd9dff3aac0a8f760e6ffb123 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 16:57:01 -0700 Subject: [PATCH 013/126] ci: updated base image --- .github/workflows/ros2.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 63c5f663..cea529fb 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -24,11 +24,11 @@ jobs: ros_distro: [humble, iron, jazzy] include: - ros_distro: 'humble' - docker_image: ubuntu:jammy + docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-desktop - ros_distro: 'iron' - docker_image: ubuntu:jammy + docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-desktop - ros_distro: 'jazzy' - docker_image: ubuntu:noble + docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-desktop fail-fast: false container: image: ${{ matrix.docker_image }} From 69c43300ab0c2e6a9b84d5d506537dfa497ff525 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 17:00:51 -0700 Subject: [PATCH 014/126] ci: updated base image --- .github/workflows/ros2.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index cea529fb..9815989d 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -24,11 +24,11 @@ jobs: ros_distro: [humble, iron, jazzy] include: - ros_distro: 'humble' - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-desktop + docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-desktop-latest - ros_distro: 'iron' - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-desktop + docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-desktop-latest - ros_distro: 'jazzy' - docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-desktop + docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-desktop-latest fail-fast: false container: image: ${{ matrix.docker_image }} From 613b73f866d41a10cd8a31ad72a51b28565770af Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Jun 2024 17:10:19 -0700 Subject: [PATCH 015/126] fix echo -e with sh --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 9815989d..6f9557f7 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,7 +38,7 @@ jobs: with: required-ros-distributions: ${{ matrix.ros_distro }} - run: | - echo -e "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\ + echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\ cat /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.3 with: From 54f9ec9c395f9852a503b5542021170be014beb5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Jun 2024 10:49:52 -0700 Subject: [PATCH 016/126] Fixed diagnostic clock with sim time. Updated docker example with explicit OMP_WAIT_POLICY=passive to avoid high CPU usage. Added log_level argument to rtabmap.launch.py. --- docker/README.md | 2 ++ rtabmap_launch/launch/rtabmap.launch.py | 16 +++++++++++----- .../include/rtabmap_sync/SyncDiagnostic.h | 16 ++++++++-------- 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/docker/README.md b/docker/README.md index 7774f8a2..8af5659c 100644 --- a/docker/README.md +++ b/docker/README.md @@ -13,6 +13,7 @@ docker run -it --rm \ --user $UID \ -e ROS_HOME=/tmp/.ros \ + -e OMP_WAIT_POLICY=passive \ --network=host \ --ipc=host \ -v ~/.ros:/tmp/.ros \ @@ -35,6 +36,7 @@ -e NVIDIA_VISIBLE_DEVICES=all \ -e NVIDIA_DRIVER_CAPABILITIES=all \ -e XAUTHORITY=$XAUTH \ + -e OMP_WAIT_POLICY=passive \ --user $UID \ -e ROS_HOME=/tmp/.ros \ -v ~/.ros:/tmp/.ros \ diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 50db0297..29a856b1 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -52,6 +52,8 @@ def launch_setup(context, *args, **kwargs): DeclareLaunchArgument('qos_user_data', default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_imu', default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_gps', default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'), + + DeclareLaunchArgument('odom_log_level', default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'), #These arguments should not be modified directly, see referred topics without "_relay" suffix above DeclareLaunchArgument('rgb_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('rgb_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('rgb_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'), @@ -182,7 +184,7 @@ def launch_setup(context, *args, **kwargs): ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rgbd_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['rgbd_odometry:=', LaunchConfiguration('odom_log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -217,7 +219,7 @@ def launch_setup(context, *args, **kwargs): ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -246,7 +248,7 @@ def launch_setup(context, *args, **kwargs): ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -290,6 +292,7 @@ def launch_setup(context, *args, **kwargs): "Mem/InitWMWithAllNodes": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' == 'true'"]))._predicate_func(context)).perform(context) }], remappings=[ + ("map", LaunchConfiguration('map_topic')), ("rgb/image", LaunchConfiguration('rgb_topic_relay')), ("depth/image", LaunchConfiguration('depth_topic_relay')), ("rgb/camera_info", LaunchConfiguration('camera_info_topic')), @@ -307,7 +310,7 @@ def launch_setup(context, *args, **kwargs): ("fiducial_transforms", LaunchConfiguration('fiducial_topic')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args")], + arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -346,7 +349,7 @@ def launch_setup(context, *args, **kwargs): ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), ("odom", LaunchConfiguration('odom_topic'))], condition=IfCondition(LaunchConfiguration("rtabmap_viz")), - arguments=[LaunchConfiguration("gui_cfg")], + arguments=[LaunchConfiguration("gui_cfg"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap_viz:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap_viz:=', LaunchConfiguration('log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), Node( @@ -391,6 +394,8 @@ def generate_launch_description(): DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), + DeclareLaunchArgument('log_level', default_value='info', description="ROS logging level (debug, info, warn, error). For RTAB-Map\'s logger level, use \"args\" argument."), + # Config files DeclareLaunchArgument('cfg', default_value='', description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'), DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini', description='Configuration path of rtabmap_viz.'), @@ -399,6 +404,7 @@ def generate_launch_description(): DeclareLaunchArgument('frame_id', default_value='base_link', description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'), DeclareLaunchArgument('odom_frame_id', default_value='', description='If set, TF is used to get odometry instead of the topic.'), DeclareLaunchArgument('map_frame_id', default_value='map', description='Output map frame id (TF).'), + DeclareLaunchArgument('map_topic', default_value='map', description='Map topic name.'), DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'), DeclareLaunchArgument('namespace', default_value='rtabmap', description=''), DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'), diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 557f80b6..7e513323 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -16,14 +16,14 @@ namespace rtabmap_sync { class SyncDiagnostic { public: SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) : - node_(node), - diagnosticUpdater_(node), - frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)), - timeStampStatus_(diagnostic_updater::TimeStampStatusParam()), - compositeTask_("Sync status"), - lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), - targetFrequency_(0.0), - windowSize_(windowSize) + node_(node), + diagnosticUpdater_(node), + frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance), node->get_clock()), + timeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), + compositeTask_("Sync status"), + lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), + targetFrequency_(0.0), + windowSize_(windowSize) { UASSERT(windowSize_ >= 1); } From ead695d2b8cebe401afeb3ae668e22c8bcf0d6ac Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Jun 2024 10:54:00 -0700 Subject: [PATCH 017/126] CI: updated ros2 workflow description --- .github/workflows/ros2.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 6f9557f7..2ae5dce3 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -17,21 +17,21 @@ jobs: # 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 on ros2 ${{ matrix.ros_distro }} and ${{ matrix.docker_image }} + name: Build on ros2 ${{ matrix.ros_distro }} on ubuntu ${{ matrix.ubuntu_distro }} runs-on: ubuntu-latest strategy: matrix: ros_distro: [humble, iron, jazzy] include: - ros_distro: 'humble' - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-desktop-latest + ubuntu_distro: 'jammy' - ros_distro: 'iron' - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-desktop-latest + ubuntu_distro: 'jammy' - ros_distro: 'jazzy' - docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-desktop-latest + ubuntu_distro: 'noble' fail-fast: false container: - image: ${{ matrix.docker_image }} + image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.docker_image }}-desktop-latest steps: - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 From 4a5430c467fc262eb708dbec67752c113d3a355b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Jun 2024 10:55:19 -0700 Subject: [PATCH 018/126] CI typo --- .github/workflows/ros2.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 2ae5dce3..b1a33bb0 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -17,7 +17,7 @@ jobs: # 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 on ros2 ${{ matrix.ros_distro }} on ubuntu ${{ matrix.ubuntu_distro }} + name: Build ros2 ${{ matrix.ros_distro }} on ubuntu ${{ matrix.ubuntu_distro }} runs-on: ubuntu-latest strategy: matrix: @@ -31,7 +31,7 @@ jobs: ubuntu_distro: 'noble' fail-fast: false container: - image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.docker_image }}-desktop-latest + image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.ros_distro }}-desktop-latest steps: - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 From 12489b3fbed5da68f71809888f3be96dd57c4d45 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 3 Jun 2024 20:07:47 -0700 Subject: [PATCH 019/126] Enabled jazzy-latest docker --- .github/workflows/docker-ros2.yml | 12 +++++++++++- .github/workflows/ros2.yml | 5 +++-- docker/jazzy/Dockerfile | 7 +++++++ docker/jazzy/latest/Dockerfile | 20 ++++++++++++++++++++ 4 files changed, 41 insertions(+), 3 deletions(-) create mode 100644 docker/jazzy/Dockerfile create mode 100644 docker/jazzy/latest/Dockerfile diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 27033771..60e114c3 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -11,7 +11,7 @@ jobs: strategy: matrix: - docker_tag: [humble, humble-latest, iron, iron-latest] + docker_tag: [humble, humble-latest, iron, iron-latest, jazzy-latest] include: - docker_tag: humble docker_path: 'humble' @@ -30,6 +30,16 @@ jobs: docker_path: 'iron/latest' docker_platforms: | linux/amd64 +# Re-add "jazzy" after binaries are released +# - docker_tag: jazzy +# docker_path: 'jazzy' +# docker_platforms: | +# linux/amd64 + - docker_tag: jazzy-latest + docker_path: 'jazzy/latest' + docker_platforms: | + linux/amd64 + linux/arm64 steps: - diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index b1a33bb0..8b33446e 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -27,8 +27,9 @@ jobs: ubuntu_distro: 'jammy' - ros_distro: 'iron' ubuntu_distro: 'jammy' - - ros_distro: 'jazzy' - ubuntu_distro: 'noble' +# Disabled as there still missing dependencies on jazzy: +# - 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 diff --git a/docker/jazzy/Dockerfile b/docker/jazzy/Dockerfile new file mode 100644 index 00000000..2de694ec --- /dev/null +++ b/docker/jazzy/Dockerfile @@ -0,0 +1,7 @@ +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 \ + && rm -rf /var/lib/apt/lists/ diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile new file mode 100644 index 00000000..09042941 --- /dev/null +++ b/docker/jazzy/latest/Dockerfile @@ -0,0 +1,20 @@ +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="nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ + apt remove ros-$ROS_DISTRO-rtabmap* -y && \ + 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 From a8cefa9a4dddc8bc42b1a99a596928fbd16c9420 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Jun 2024 11:32:27 -0700 Subject: [PATCH 020/126] yaml_to_camera_info: support yaml format writen by opencv --- rtabmap_util/scripts/yaml_to_camera_info.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rtabmap_util/scripts/yaml_to_camera_info.py b/rtabmap_util/scripts/yaml_to_camera_info.py index ab8a8b25..ace42253 100755 --- a/rtabmap_util/scripts/yaml_to_camera_info.py +++ b/rtabmap_util/scripts/yaml_to_camera_info.py @@ -7,6 +7,9 @@ from sensor_msgs.msg import Image def yaml_to_CameraInfo(yaml_fname): with open(yaml_fname, "r") as file_handle: + first_line = file_handle.readline() + if "%YAML:" not in first_line: + file_handle.seek(0) calib_data = yaml.load(file_handle, Loader=yaml.FullLoader) camera_info_msg = CameraInfo() From c3e27cfaf513a80d44e8d6a8fd7f8c477453b831 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Jun 2024 11:33:01 -0700 Subject: [PATCH 021/126] point_cloud_xyzrgb: add support for SGBM parameters --- rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index f17bc704..b57075ca 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -161,6 +161,8 @@ private: // StereoBM parameters stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM"); + uInsert(stereoBMParameters_, rtabmap::Parameters::getDefaultParameters("StereoSGBM")); + stereoBMParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kStereoDenseStrategy(), uNumber2Str(rtabmap::Parameters::defaultStereoDenseStrategy()))); for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter) { std::string vStr; From 9e3c746e11b63c117a20f48ccac50a64bcf28ffe Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Jun 2024 11:43:14 -0700 Subject: [PATCH 022/126] jazzy: fixed docker error --- docker/jazzy/latest/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 09042941..74c02aec 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -13,7 +13,6 @@ RUN source /ros_entrypoint.sh && \ 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="nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ - apt remove ros-$ROS_DISTRO-rtabmap* -y && \ 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 && \ From b8741910e4133252764aa9d8ffc4b811c99754b9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Jun 2024 18:19:29 -0700 Subject: [PATCH 023/126] Update Dockerfile --- docker/jazzy/latest/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 74c02aec..a6c112e2 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -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="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 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 && \ From 911e4e89412c9658d569c0258730cb386b493d77 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Jun 2024 22:42:06 -0700 Subject: [PATCH 024/126] Making nav2_msgs optional (not available yet on jazzy). Fixed compilation errors on jazzy with vision_opencv related h->hpp headers. --- rtabmap_conversions/CMakeLists.txt | 8 ++++++ .../rtabmap_conversions/MsgConversion.h | 4 +++ rtabmap_conversions/src/MsgConversion.cpp | 7 ++++- .../include/rtabmap_odom/rgbd_odometry.hpp | 4 +++ .../include/rtabmap_odom/stereo_odometry.hpp | 4 +++ rtabmap_odom/src/OdometryROS.cpp | 4 +++ rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 4 +++ rtabmap_odom/src/nodelets/stereo_odometry.cpp | 4 +++ rtabmap_rviz_plugins/CMakeLists.txt | 26 +++-------------- rtabmap_slam/CMakeLists.txt | 22 ++++++++++----- .../include/rtabmap_slam/CoreWrapper.h | 8 ++++++ rtabmap_slam/src/CoreWrapper.cpp | 28 +++++++++++++++++-- .../rtabmap_sync/CommonDataSubscriber.h | 4 +++ .../src/impl/CommonDataSubscriberRGBD.cpp | 1 - .../src/impl/CommonDataSubscriberRGBD2.cpp | 1 - .../src/impl/CommonDataSubscriberRGBD3.cpp | 3 +- .../src/impl/CommonDataSubscriberRGBD4.cpp | 1 - .../src/impl/CommonDataSubscriberRGBD5.cpp | 1 - .../src/impl/CommonDataSubscriberRGBD6.cpp | 1 - .../src/impl/CommonDataSubscriberRGBDX.cpp | 1 - .../impl/CommonDataSubscriberSensorData.cpp | 1 - rtabmap_sync/src/nodelets/rgb_sync.cpp | 4 +++ rtabmap_sync/src/nodelets/rgbd_sync.cpp | 4 +++ rtabmap_sync/src/nodelets/stereo_sync.cpp | 4 +++ rtabmap_util/src/DbPlayerNode.cpp | 4 +++ .../src/nodelets/disparity_to_depth.cpp | 4 +++ rtabmap_util/src/nodelets/imu_to_tf.cpp | 2 +- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 10 +++++-- .../src/nodelets/point_cloud_xyzrgb.cpp | 8 +++++- rtabmap_util/src/nodelets/rgbd_relay.cpp | 4 +++ rtabmap_util/src/nodelets/rgbd_split.cpp | 4 +++ 31 files changed, 139 insertions(+), 46 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 27bb3b22..3dd23609 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -46,6 +46,10 @@ if("$ENV{ROS_DISTRO}" STRLESS "humble") add_definitions(-DPRE_ROS_HUMBLE) endif() +IF("$ENV{ROS_DISTRO}" STRLESS "iron") + add_definitions(-DPRE_ROS_IRON) +ENDIF() + ########### ## Build ## ########### @@ -58,6 +62,10 @@ target_include_directories(rtabmap_conversions ) ament_target_dependencies(rtabmap_conversions ${Libraries}) +IF("$ENV{ROS_DISTRO}" STRLESS "iron") + target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON) +ENDIF() + ############# ## Install ## ############# diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 362a010f..904731c2 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -40,7 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 1587d06a..9c465599 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -38,8 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#ifdef PRE_ROS_IRON #include #include +#else +#include +#include +#endif #include #include #include @@ -47,7 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #ifdef PRE_ROS_HUMBLE #include -#include +#include #else #include #include diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 090c3beb..9bff1cbf 100644 --- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp @@ -41,7 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif namespace rtabmap_odom { diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index 3f5dddf4..b66a201b 100644 --- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp @@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include #include diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 0b8235e5..201fff0b 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -34,7 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index b81e37a5..f608cc07 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -27,7 +27,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index c249ac40..20a2ff23 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -29,7 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include "rtabmap_conversions/MsgConversion.h" #include diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index 95cf2f69..d466c1b2 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -46,21 +46,6 @@ MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}") ## We also use Ogre for rviz plugins include_directories( ${OGRE_INCLUDE_DIRS} ) -## RVIZ plugin -IF(QT4_FOUND) - qt4_wrap_cpp(MOC_FILES - include/${PROJECT_NAME}/MapCloudDisplay.h - include/${PROJECT_NAME}/MapGraphDisplay.h - include/${PROJECT_NAME}/InfoDisplay.h - ) -ELSE() - qt5_wrap_cpp(MOC_FILES - include/${PROJECT_NAME}/MapCloudDisplay.h - include/${PROJECT_NAME}/MapGraphDisplay.h - include/${PROJECT_NAME}/InfoDisplay.h - ) -ENDIF() - # tf:message_filters, mixing boost and Qt signals set_property( SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp src/OrbitOrientedViewController.cpp @@ -71,8 +56,11 @@ add_library(rtabmap_rviz_plugins SHARED src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp - ${MOC_FILES} + include/${PROJECT_NAME}/MapCloudDisplay.h + include/${PROJECT_NAME}/MapGraphDisplay.h + include/${PROJECT_NAME}/InfoDisplay.h ) +set_property(TARGET rtabmap_rviz_plugins PROPERTY AUTOMOC ON) target_include_directories(rtabmap_rviz_plugins PUBLIC $ @@ -81,10 +69,6 @@ target_include_directories(rtabmap_rviz_plugins ament_target_dependencies(rtabmap_rviz_plugins ${Libraries}) -IF(Qt5_FOUND) - QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui) -ENDIF(Qt5_FOUND) - # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY") @@ -111,6 +95,4 @@ install(TARGETS INCLUDES DESTINATION include ) -pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) - ament_package() diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index 946180d9..ae9c6a84 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) -find_package(nav2_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -28,12 +27,9 @@ find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_util REQUIRED) find_package(rtabmap_sync REQUIRED) -IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0) - ADD_DEFINITIONS("-DNAV_MSGS_FOXY") -ENDIF() - #optional find_package(apriltag_msgs) +find_package(nav2_msgs) IF(WIN32) add_compile_options(-bigobj) @@ -48,7 +44,6 @@ SET(Libraries cv_bridge geometry_msgs nav_msgs - nav2_msgs rclcpp rclcpp_components sensor_msgs @@ -80,6 +75,19 @@ SET(Libraries ) ENDIF(apriltag_msgs_FOUND) +# If nav2_msgs is found, add definition +IF(nav2_msgs_FOUND) +MESSAGE(STATUS "WITH nav2_msgs") +ADD_DEFINITIONS("-DWITH_NAV2_MSGS") +SET(Libraries + ${Libraries} + nav2_msgs +) +IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0) + ADD_DEFINITIONS("-DNAV_MSGS_FOXY") +ENDIF() +ENDIF(nav2_msgs_FOUND) + ############################ ## Declare a cpp library ############################ @@ -128,4 +136,4 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) -ament_package() \ No newline at end of file +ament_package() diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index bca24fdb..7ac6825c 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -88,8 +88,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef WITH_NAV2_MSGS #include #include +#endif //#define WITH_FIDUCIAL_MSGS #ifdef WITH_FIDUCIAL_MSGS @@ -109,8 +111,10 @@ public: explicit CoreWrapper(const rclcpp::NodeOptions & options); virtual ~CoreWrapper(); +#ifdef WITH_NAV2_MSGS using NavigateToPose = nav2_msgs::action::NavigateToPose; using GoalHandleNav2 = rclcpp_action::ClientGoalHandle; +#endif private: bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp); @@ -246,12 +250,14 @@ private: void publishStats(const rclcpp::Time & stamp); void publishCurrentGoal(const rclcpp::Time & stamp); +#ifdef WITH_NAV2_MSGS #ifdef NAV_MSGS_FOXY void goalResponseCallback(std::shared_future future); #else void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle); #endif void resultCallback(const GoalHandleNav2::WrappedResult & result); +#endif void publishLocalPath(const rclcpp::Time & stamp); void publishGlobalPath(const rclcpp::Time & stamp); @@ -373,7 +379,9 @@ private: rclcpp::Service::SharedPtr octomapBinarySrv_; rclcpp::Service::SharedPtr octomapFullSrv_; #endif +#ifdef WITH_NAV2_MSGS rclcpp_action::Client::SharedPtr nav2Client_; +#endif std::thread* transformThread_; bool tfThreadRunning_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 89a7b01a..17c46d58 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -36,9 +36,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include +#if PCL_VERSION_COMPARE(>, 1, 12, 0) +#include +#else #include +#endif #include @@ -196,6 +204,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr); useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_); +#ifndef WITH_NAV2_MSGS + if(useActionForGoal_) + { + RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support."); + } +#endif useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_); genScan_ = this->declare_parameter("gen_scan", genScan_); genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_); @@ -2199,7 +2213,11 @@ void CoreWrapper::process( { // Don't send status yet if nav2 actionlib is used unless it failed, // let nav2 finish reaching the goal +#ifdef WITH_NAV2_MSGS if(nav2Client_ == 0 || rtabmap_.getPathStatus() <= 0) +#else + if(rtabmap_.getPathStatus() <= 0) +#endif { if(rtabmap_.getPathStatus() > 0) { @@ -2209,10 +2227,12 @@ void CoreWrapper::process( else if(rtabmap_.getPathStatus() <= 0) { RCLCPP_WARN(this->get_logger(), "Planning: Plan failed!"); +#ifdef WITH_NAV2_MSGS if(nav2Client_.get()!=NULL && nav2Client_->action_server_is_ready()) { nav2Client_->async_cancel_all_goals(); } +#endif } if(goalReachedPub_->get_subscription_count()) @@ -3964,11 +3984,12 @@ void CoreWrapper::cancelGoalCallback( goalReachedPub_->publish(result); } } - +#ifdef WITH_NAV2_MSGS if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready()) { nav2Client_->async_cancel_all_goals(); } +#endif } void CoreWrapper::setLabelCallback( @@ -4364,6 +4385,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp) poseMsg.header.frame_id = mapFrameId_; poseMsg.header.stamp = stamp; rtabmap_conversions::transformToPoseMsg(currentMetricGoal_, poseMsg.pose); +#ifdef WITH_NAV2_MSGS if(useActionForGoal_) { if(nav2Client_.get() == NULL || !nav2Client_->action_server_is_ready()) @@ -4395,6 +4417,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp) RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!"); } } +#endif if(nextMetricGoalPub_->get_subscription_count()) { nextMetricGoalPub_->publish(poseMsg); @@ -4405,7 +4428,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp) } } } - +#ifdef WITH_NAV2_MSGS void CoreWrapper::goalResponseCallback( #ifdef NAV_MSGS_FOXY std::shared_future future) @@ -4473,6 +4496,7 @@ void CoreWrapper::resultCallback( latestNodeWasReached_ = false; } } +#endif void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp) { diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index a2e9c681..f7ff4d50 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index 43c26f91..2666223e 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index 21f79374..18fd1f35 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index 7b62912a..58a22cc7 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -28,8 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include -#include "../../../rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h" +#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index 1977a49b..87a4a8d1 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index ac588cbd..dac97cf0 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index 10c32560..e1a04566 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index e2e10a49..3c66d1ff 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index e9f3b4f2..b9c49cd1 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 70f5cfb9..1ed31f96 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include "rtabmap/core/Compression.h" diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 076ee4c0..30463852 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include "rtabmap/core/Compression.h" diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index a5c78f0f..b52e6803 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include "rtabmap/core/Compression.h" diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 80d9eb8a..0650d810 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -36,7 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include #include diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index 46f62561..7beb7c8b 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif namespace rtabmap_util { diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 4cb9f056..c079c8ea 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include namespace rtabmap_util diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index f92efbbf..bc8fabd0 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -30,10 +30,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include - -#include - +#ifdef PRE_ROS_IRON #include +#include +#else +#include +#include +#endif + #include #include "rtabmap/core/util2d.h" diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index e228e3f5..6130df32 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -31,10 +31,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON +#include #include #include +#else +#include +#include +#include +#endif -#include #include #include "rtabmap/core/util2d.h" diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index f33add2a..ce5d8e57 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -32,7 +32,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include "rtabmap_conversions/MsgConversion.h" diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index a7cd3417..fe291e0e 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -27,7 +27,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif namespace rtabmap_util { From 9ced85d5f8aee79e5d8276f4000b0c742ef6f5a6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Jun 2024 21:45:33 -0700 Subject: [PATCH 025/126] Added dev container --- .devcontainer/devcontainer.json | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .devcontainer/devcontainer.json diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..36648e61 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,11 @@ +{ + "image": "introlab3it/rtabmap_ros:noetic-latest", + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"] + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/catkin_ws", + "postAttachCommand": "apt remove -y ros-noetic-rtabmap-* && echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'" +} From ae44e1a2157b84e67ade530f2b8e713eac9650e8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Jun 2024 22:35:54 -0700 Subject: [PATCH 026/126] Split queue_size param into sync_queue_size and topic_queue_size parameters for more fine tuning of topic synchronization (#1054) --- rtabmap_launch/launch/rtabmap.launch | 25 +++- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 93 +++++++----- .../src/nodelets/rgbdicp_odometry.cpp | 47 ++++-- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 95 +++++++----- .../rtabmap_sync/CommonDataSubscriber.h | 58 +++----- .../CommonDataSubscriberDefines.h | 2 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 78 +++++----- .../src/impl/CommonDataSubscriberDepth.cpp | 138 +++++++++--------- .../src/impl/CommonDataSubscriberOdom.cpp | 20 ++- .../src/impl/CommonDataSubscriberRGB.cpp | 136 +++++++++-------- .../src/impl/CommonDataSubscriberRGBD.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD2.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD3.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD4.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD5.cpp | 44 +++--- .../src/impl/CommonDataSubscriberRGBD6.cpp | 44 +++--- .../src/impl/CommonDataSubscriberRGBDX.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberScan.cpp | 86 ++++++----- .../impl/CommonDataSubscriberSensorData.cpp | 20 ++- .../src/impl/CommonDataSubscriberStereo.cpp | 26 ++-- rtabmap_sync/src/nodelets/rgb_sync.cpp | 28 +++- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 30 +++- rtabmap_sync/src/nodelets/rgbdx_sync.cpp | 34 +++-- rtabmap_sync/src/nodelets/stereo_sync.cpp | 32 ++-- .../src/nodelets/obstacles_detection.cpp | 2 - .../src/nodelets/point_cloud_aggregator.cpp | 27 +++- .../src/nodelets/point_cloud_assembler.cpp | 34 +++-- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 33 +++-- .../src/nodelets/point_cloud_xyzrgb.cpp | 45 ++++-- .../src/nodelets/pointcloud_to_depthimage.cpp | 28 +++- rtabmap_util/src/nodelets/rgbd_relay.cpp | 5 - rtabmap_util/src/nodelets/rgbd_split.cpp | 5 - rtabmap_viz/src/GuiWrapper.cpp | 16 +- 33 files changed, 882 insertions(+), 779 deletions(-) diff --git a/rtabmap_launch/launch/rtabmap.launch b/rtabmap_launch/launch/rtabmap.launch index 3cd5a90d..19ea0b14 100644 --- a/rtabmap_launch/launch/rtabmap.launch +++ b/rtabmap_launch/launch/rtabmap.launch @@ -47,7 +47,9 @@ - + + + @@ -181,7 +183,8 @@ - + + @@ -203,7 +206,8 @@ - + + @@ -253,7 +257,8 @@ - + + @@ -283,7 +288,8 @@ - + + @@ -310,7 +316,8 @@ - + + @@ -377,7 +384,8 @@ - + + @@ -434,7 +442,8 @@ - + + diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index f1932c7d..bda1e1a9 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -74,7 +74,8 @@ public: exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(1), + syncQueueSize_(5), keepColor_(false) { } @@ -110,7 +111,19 @@ private: double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); if(pnh.hasParam("depth_cameras")) { @@ -126,7 +139,8 @@ private: NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_); + NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras); NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); @@ -137,23 +151,23 @@ private: { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); - rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_); } if(rgbdCameras == 2) @@ -161,7 +175,7 @@ private: if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -171,7 +185,7 @@ private: else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -188,7 +202,7 @@ private: if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -199,7 +213,7 @@ private: else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -218,7 +232,7 @@ private: if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -230,7 +244,7 @@ private: else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -251,7 +265,7 @@ private: if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -264,7 +278,7 @@ private: else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -287,7 +301,7 @@ private: if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -301,7 +315,7 @@ private: else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -333,7 +347,7 @@ private: } else if(rgbdCameras == 0) { - rgbdxSub_ = nh.subscribe("rgbd_images", 1, &RGBDOdometry::callbackRGBDX, this); + rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &RGBDOdometry::callbackRGBDX, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -342,7 +356,7 @@ private: } else { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this); + rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &RGBDOdometry::callbackRGBD, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -361,20 +375,20 @@ private: image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); if(approxSyncMaxInterval > 0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } @@ -758,20 +772,20 @@ protected: if(approxSync_) { delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } if(exactSync_) { delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -780,7 +794,7 @@ protected: { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -789,7 +803,7 @@ protected: { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -799,7 +813,7 @@ protected: { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -809,7 +823,7 @@ protected: { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -820,7 +834,7 @@ protected: { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -831,7 +845,7 @@ protected: { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -843,7 +857,7 @@ protected: { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -855,7 +869,7 @@ protected: { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -868,7 +882,7 @@ protected: { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -917,7 +931,8 @@ private: message_filters::Synchronizer * approxSync6_; typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; message_filters::Synchronizer * exactSync6_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; bool keepColor_; }; diff --git a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp index 65e9b8cc..99b5ddb1 100644 --- a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp @@ -72,7 +72,8 @@ public: exactScanSync_(0), approxCloudSync_(0), exactCloudSync_(0), - queueSize_(5), + queueSize_(1), + syncQueueSize_(5), keepColor_(false), scanCloudMaxPoints_(0), scanVoxelSize_(0.0), @@ -113,7 +114,19 @@ private: double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", queueSize_, queueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); @@ -130,7 +143,8 @@ private: NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDIcpOdometry: topic_queue_size = %d", queueSize_); + NODELET_INFO("RGBDIcpOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_); @@ -147,24 +161,24 @@ private: image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize_, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize_, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", queueSize_); std::string subscribedTopicsMsg; if(subscribeScanCloud) { - cloud_sub_.subscribe(nh, "scan_cloud", 1); + cloud_sub_.subscribe(nh, "scan_cloud", queueSize_); if(approxSync) { - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); if(approxSyncMaxInterval > 0.0) approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -179,17 +193,17 @@ private: } else { - scan_sub_.subscribe(nh, "scan", 1); + scan_sub_.subscribe(nh, "scan", queueSize_); if(approxSync) { - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); if(approxSyncMaxInterval > 0.0) approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -460,25 +474,25 @@ protected: if(approxScanSync_) { delete approxScanSync_; - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactScanSync_) { delete exactScanSync_; - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(approxCloudSync_) { delete approxCloudSync_; - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactCloudSync_) { delete exactCloudSync_; - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } } @@ -498,6 +512,7 @@ private: typedef message_filters::sync_policies::ApproximateTime MyExactCloudSyncPolicy; message_filters::Synchronizer * exactCloudSync_; int queueSize_; + int syncQueueSize_; bool keepColor_; int scanCloudMaxPoints_; double scanVoxelSize_; diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 889de365..e698c38d 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -74,7 +74,8 @@ public: exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(1), + syncQueueSize_(5), keepColor_(false) { } @@ -109,7 +110,19 @@ private: int rgbdCameras = 1; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); pnh.param("keep_color", keepColor_, keepColor_); @@ -117,7 +130,8 @@ private: NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_); + NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_); + NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false"); @@ -127,23 +141,23 @@ private: { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); - rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_); } if(rgbdCameras == 2) @@ -151,7 +165,7 @@ private: if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -161,7 +175,7 @@ private: else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -178,7 +192,7 @@ private: if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -189,7 +203,7 @@ private: else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -208,7 +222,7 @@ private: if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -220,7 +234,7 @@ private: else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -241,7 +255,7 @@ private: if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -254,7 +268,7 @@ private: else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -277,7 +291,7 @@ private: if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -291,7 +305,7 @@ private: else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -323,7 +337,7 @@ private: } else if(rgbdCameras == 0) { - rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this); + rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &StereoOdometry::callbackRGBDX, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -332,7 +346,7 @@ private: } else { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); + rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &StereoOdometry::callbackRGBD, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -351,21 +365,21 @@ private: image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), topicQueueSize_, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), topicQueueSize_, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_); + cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_); if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); if(approxSyncMaxInterval>0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -897,20 +911,20 @@ protected: if(approxSync_) { delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactSync_) { delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -919,7 +933,7 @@ protected: { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -928,7 +942,7 @@ protected: { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -938,7 +952,7 @@ protected: { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -948,7 +962,7 @@ protected: { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -959,7 +973,7 @@ protected: { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -970,7 +984,7 @@ protected: { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -982,7 +996,7 @@ protected: { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -994,7 +1008,7 @@ protected: { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1007,7 +1021,7 @@ protected: { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1058,7 +1072,8 @@ private: typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; message_filters::Synchronizer * exactSync6_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; bool keepColor_; }; diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 6c486fcb..79b6127a 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -74,7 +74,8 @@ public: bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;} bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();} int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;} - int getQueueSize() const {return queueSize_;} + int getTopicQueueSize() const {return topicQueueSize_;} + int getSyncQueueSize() const {return syncQueueSize_;} bool isApproxSync() const {return approxSync_;} const std::string & name() const {return name_;} @@ -140,16 +141,12 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupStereoCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -158,9 +155,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -169,9 +164,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDXCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -180,9 +173,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #ifdef RTABMAP_SYNC_MULTI_RGBD void setupRGBD2Callbacks( ros::NodeHandle & nh, @@ -192,9 +183,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD3Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -203,9 +192,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD4Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -214,9 +201,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD5Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -225,9 +210,7 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD6Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -236,17 +219,13 @@ private: bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #endif void setupSensorDataCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupScanCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -254,20 +233,17 @@ private: bool subscribeScanDesc, bool subscribeOdom, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupOdomCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); protected: std::string subscribedTopicsMsg_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; private: bool approxSync_; diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h index e614c5c2..a8938ba1 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h @@ -180,7 +180,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. } \ subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \ name_.c_str(), \ - approxSync?"approx":"exact", \ + APPROX?"approx":"exact", \ SUB0.getTopic().c_str(), \ SUB1.getTopic().c_str(), \ SUB2.getTopic().c_str(), \ diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 0960064d..e95597ed 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -30,7 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(bool gui) : - queueSize_(10), + topicQueueSize_(10), + syncQueueSize_(10), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -504,11 +505,23 @@ void CommonDataSubscriber::setupCallbacks( { ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true."); } - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) { ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " - "to \"approx_sync\"! Your value is still copied to " + "to \"approx_sync\"! Your value is copied to " "corresponding parameter."); pnh.param("stereo_approx_sync", approxSync_, approxSync_); } @@ -527,8 +540,9 @@ void CommonDataSubscriber::setupCallbacks( ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false"); ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false"); ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false"); - ROS_INFO("%s: queue_size = %d", name.c_str(), queueSize_); - ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); + ROS_INFO("%s: topic_queue_size = %d", name.c_str(), topicQueueSize_); + ROS_INFO("%s: sync_queue_size = %d", name.c_str(), syncQueueSize_); + ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); subscribedToOdom_ = odomFrameId.empty() && subscribeOdom; if(subscribedToDepth_) @@ -541,9 +555,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToStereo_) { @@ -551,9 +563,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribedToOdom_, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToRGB_) { @@ -565,9 +575,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToRGBD_) { @@ -589,9 +597,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 5) { @@ -603,9 +609,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 4) { @@ -617,9 +621,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 3) { @@ -631,9 +633,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 2) { @@ -645,9 +645,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } #else if(rgbdCameras>1) @@ -668,9 +666,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else { @@ -682,9 +678,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } } else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc) @@ -696,9 +690,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScanDesc, subscribedToOdom_, subscribeUserData, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToSensorData_) { @@ -706,9 +698,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribedToOdom_, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToOdom_) { @@ -716,9 +706,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribeUserData, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_) @@ -732,7 +720,7 @@ void CommonDataSubscriber::setupCallbacks( "the clocks of the computers are synchronized (\"ntpdate\"). %s%s", name_.c_str(), approxSync_? - uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str(): + uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str(): "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()), otherTasks); diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 0b2dc897..b2494c23 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -463,9 +463,7 @@ void CommonDataSubscriber::setupDepthCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup depth callback"); @@ -480,195 +478,195 @@ void CommonDataSubscriber::setupDepthCallbacks( image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } #endif @@ -677,57 +675,57 @@ void CommonDataSubscriber::setupDepthCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp index d4cab009..78623517 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -67,29 +67,27 @@ void CommonDataSubscriber::setupOdomCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup scan callback"); if(subscribeUserData || subscribeOdomInfo) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_); + SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_); } } else @@ -97,13 +95,13 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_); } } else { - odomSubOnly_ = nh.subscribe("odom", queueSize, &CommonDataSubscriber::odomCallback, this); + odomSubOnly_ = nh.subscribe("odom", syncQueueSize_, &CommonDataSubscriber::odomCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index 5e6867b7..30b2dcec 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -463,9 +463,7 @@ void CommonDataSubscriber::setupRGBCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgb-only callback"); @@ -475,194 +473,194 @@ void CommonDataSubscriber::setupRGBCallbacks( image_transport::ImageTransport rgb_it(rgb_nh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), syncQueueSize_, hintsRgb); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_); } } #endif @@ -671,57 +669,57 @@ void CommonDataSubscriber::setupRGBCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_); + SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index e620b448..582809b6 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -539,9 +539,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd callback"); @@ -556,152 +554,152 @@ void CommonDataSubscriber::setupRGBDCallbacks( { rgbdSubs_.resize(1); rgbdSubs_[0] = new message_filters::Subscriber; - rgbdSubs_[0]->subscribe(nh, "rgbd_image", queueSize); + rgbdSubs_[0]->subscribe(nh, "rgbd_image", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0])); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0])); + SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); + SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0])); } } #endif @@ -710,41 +708,41 @@ void CommonDataSubscriber::setupRGBDCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_); } else { @@ -754,7 +752,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( } else { - rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this); + rgbdSub_ = nh.subscribe("rgbd_image", syncQueueSize_, &CommonDataSubscriber::rgbdCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index adbaabcb..0fb4df15 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -350,9 +350,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd2 callback"); @@ -360,152 +358,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks( for(int i=0; i<2; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } #endif @@ -514,45 +512,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index 77c045e1..91f73d01 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -438,9 +438,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDescriptor, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd3 callback"); @@ -448,151 +446,151 @@ void CommonDataSubscriber::setupRGBD3Callbacks( for(int i=0; i<3; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } #endif @@ -601,45 +599,45 @@ void CommonDataSubscriber::setupRGBD3Callbacks( if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index b4289e22..637645ca 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -407,9 +407,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd4 callback"); @@ -417,152 +415,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks( for(int i=0; i<4; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } #endif @@ -571,45 +569,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index 1e6ceac9..c9822154 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -263,9 +263,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd5 callback"); @@ -273,53 +271,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks( for(int i=0; i<5; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); } } else @@ -327,45 +325,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index faf21b0b..7dbfcd15 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -281,9 +281,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd6 callback"); @@ -291,53 +289,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks( for(int i=0; i<6; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); } } else @@ -345,45 +343,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index 7f28041a..5e9d425b 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -326,157 +326,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbdX callback"); - rgbdXSub_.subscribe(nh, "rgbd_images", queueSize); + rgbdXSub_.subscribe(nh, "rgbd_images", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_); } } #endif @@ -485,46 +483,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync_, syncQueueSize_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync_, syncQueueSize_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync_, syncQueueSize_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_); } else { rgbdXSub_.unsubscribe(); - rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this); + rgbdXSubOnly_ = nh.subscribe("rgbd_images", syncQueueSize_, &CommonDataSubscriber::rgbdXCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp index 54950021..992b5b5e 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -250,9 +250,7 @@ void CommonDataSubscriber::setupScanCallbacks( bool scanDescTopic, bool subscribeOdom, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup scan callback"); @@ -261,36 +259,36 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); } else if(scan2dTopic) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); } else { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -298,12 +296,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_); } } else @@ -311,12 +309,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_); } } } @@ -324,19 +322,19 @@ void CommonDataSubscriber::setupScanCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_); } } else if(scan2dTopic) @@ -344,12 +342,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_); } } else @@ -357,31 +355,31 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_); } } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -389,12 +387,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_); } } else @@ -402,12 +400,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_); } } } @@ -415,18 +413,18 @@ void CommonDataSubscriber::setupScanCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); if(scanDescTopic) { - SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_); } else if(scan2dTopic) { - SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync_, syncQueueSize_, scan3dSub_, odomInfoSub_); } } } @@ -435,7 +433,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSubOnly_ = nh.subscribe("scan_descriptor", queueSize, &CommonDataSubscriber::scanDescCallback, this); + scanDescSubOnly_ = nh.subscribe("scan_descriptor", syncQueueSize_, &CommonDataSubscriber::scanDescCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), @@ -444,7 +442,7 @@ void CommonDataSubscriber::setupScanCallbacks( else if(scan2dTopic) { subscribedToScan2d_ = true; - scan2dSubOnly_ = nh.subscribe("scan", queueSize, &CommonDataSubscriber::scan2dCallback, this); + scan2dSubOnly_ = nh.subscribe("scan", syncQueueSize_, &CommonDataSubscriber::scan2dCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), @@ -453,7 +451,7 @@ void CommonDataSubscriber::setupScanCallbacks( else { subscribedToScan3d_ = true; - scan3dSubOnly_ = nh.subscribe("scan_cloud", queueSize, &CommonDataSubscriber::scan3dCallback, this); + scan3dSubOnly_ = nh.subscribe("scan_cloud", syncQueueSize_, &CommonDataSubscriber::scan3dCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index b54a7c33..5e6a7a39 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -68,25 +68,23 @@ void CommonDataSubscriber::setupSensorDataCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup SensorData callback"); - sensorDataSub_.subscribe(nh, "sensor_data", queueSize); + sensorDataSub_.subscribe(nh, "sensor_data", topicQueueSize_); if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_); + SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_); } } else @@ -94,13 +92,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_); } else { sensorDataSub_.unsubscribe(); - sensorDataSubOnly_ = nh.subscribe("sensor_data", queueSize, &CommonDataSubscriber::sensorDataCallback, this); + sensorDataSubOnly_ = nh.subscribe("sensor_data", syncQueueSize_, &CommonDataSubscriber::sensorDataCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 4c67845a..e09dbab4 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -89,9 +89,7 @@ void CommonDataSubscriber::setupStereoCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup stereo callback"); @@ -104,24 +102,24 @@ void CommonDataSubscriber::setupStereoCallbacks( image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), queueSize, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), queueSize, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize); - cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize); + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), syncQueueSize_, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), syncQueueSize_, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_); + cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_); if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); } } else @@ -129,12 +127,12 @@ void CommonDataSubscriber::setupStereoCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); } } } diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 688c3ffd..35ffc382 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -77,18 +77,32 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("compressed_rate", compressedRate_, compressedRate_); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); rgbdImagePub_ = nh.advertise("rgbd_image", 1); @@ -96,14 +110,14 @@ private: if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_); exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } @@ -112,8 +126,8 @@ private: image_transport::ImageTransport rgb_it(rgb_nh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), queueSize, hintsRgb); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index eb2665ec..70d7607d 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -81,12 +81,25 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("depth_scale", depthScale_, depthScale_); pnh.param("decimation", decimation_, decimation_); pnh.param("compressed_rate", compressedRate_, compressedRate_); @@ -99,7 +112,8 @@ private: NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_); NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); @@ -109,14 +123,14 @@ private: if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } @@ -129,9 +143,9 @@ private: image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index f68c56b3..6fb187ae 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -77,19 +77,33 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras); rgbdImagesPub_ = nh.advertise("rgbd_images", 1); @@ -107,7 +121,7 @@ private: std::string subscribedTopicsMsg_; if(rgbdCameras==2) { - SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL2(RGBDXSync, rgbd2, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -115,7 +129,7 @@ private: } else if(rgbdCameras==3) { - SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL3(RGBDXSync, rgbd3, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -123,7 +137,7 @@ private: } else if(rgbdCameras==4) { - SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL4(RGBDXSync, rgbd4, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -131,7 +145,7 @@ private: } else if(rgbdCameras==5) { - SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL5(RGBDXSync, rgbd5, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -139,7 +153,7 @@ private: } else if(rgbdCameras==6) { - SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL6(RGBDXSync, rgbd6, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -147,7 +161,7 @@ private: } else if(rgbdCameras==7) { - SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6])); + SYNC_DECL7(RGBDXSync, rgbd7, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -155,7 +169,7 @@ private: } else if(rgbdCameras==8) { - SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7])); + SYNC_DECL8(RGBDXSync, rgbd8, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index 82be9b0d..ebb6ad2c 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -77,18 +77,32 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); if(approxSync) pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("compressed_rate", compressedRate_, compressedRate_); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); rgbdImagePub_ = nh.advertise("rgbd_image", 1); @@ -96,14 +110,14 @@ private: if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); if(approxSyncMaxInterval>0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); exactSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -116,10 +130,10 @@ private: image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh); - imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb); - imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth); - cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1); - cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1); + imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), queueSize, hintsRgb); + imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), queueSize, hintsDepth); + cameraInfoLeftSub_.subscribe(left_nh, "camera_info", queueSize); + cameraInfoRightSub_.subscribe(right_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 1fd0038b..b5213276 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -113,8 +113,6 @@ private: ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 65f4a47f..80493365 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -99,11 +99,24 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 5; + int queueSize = 1; + int syncQueueSize = 5; int count = 2; bool approx=true; double approxSyncMaxInterval = 0.0; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("frame_id", frameId_, frameId_); pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("approx_sync", approx, approx); @@ -112,14 +125,14 @@ private: pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("xyz_output", xyzOutput_, xyzOutput_); - cloudSub_1_.subscribe(nh, "cloud1", 1); - cloudSub_2_.subscribe(nh, "cloud2", 1); + cloudSub_1_.subscribe(nh, "cloud1", queueSize); + cloudSub_2_.subscribe(nh, "cloud2", queueSize); std::string subscribedTopicsMsg; if(count == 4) { - cloudSub_3_.subscribe(nh, "cloud3", 1); - cloudSub_4_.subscribe(nh, "cloud4", 1); + cloudSub_3_.subscribe(nh, "cloud3", queueSize); + cloudSub_4_.subscribe(nh, "cloud4", queueSize); if(approx) { approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); @@ -143,7 +156,7 @@ private: } else if(count == 3) { - cloudSub_3_.subscribe(nh, "cloud3", 1); + cloudSub_3_.subscribe(nh, "cloud3", queueSize); if(approx) { approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 33126a12..ead6cb0e 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -109,10 +109,23 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 5; + int queueSize = 1; + int syncQueueSize = 5; bool subscribeOdomInfo = false; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("frame_id", frameId_, frameId_); pnh.param("max_clouds", maxClouds_, maxClouds_); @@ -131,7 +144,8 @@ private: pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0); - ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize); + ROS_INFO("%s: topic_queue_size=%d", getName().c_str(), queueSize); + ROS_INFO("%s: sync_queue_size=%d", getName().c_str(), syncQueueSize); ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str()); ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str()); ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_); @@ -166,10 +180,10 @@ private: } else if(subscribeOdomInfo) { - syncCloudSub_.subscribe(nh, "cloud", 1); - syncOdomSub_.subscribe(nh, "odom", 1); - syncOdomInfoSub_.subscribe(nh, "odom_info", 1); - exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); + syncCloudSub_.subscribe(nh, "cloud", queueSize); + syncOdomSub_.subscribe(nh, "odom", queueSize); + syncOdomInfoSub_.subscribe(nh, "odom_info", queueSize); + exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); exactInfoSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", getName().c_str(), @@ -181,9 +195,9 @@ private: } else { - syncCloudSub_.subscribe(nh, "cloud", 1); - syncOdomSub_.subscribe(nh, "odom", 1); - exactSync_ = new message_filters::Synchronizer(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_); + syncCloudSub_.subscribe(nh, "cloud", queueSize); + syncOdomSub_.subscribe(nh, "odom", queueSize); + exactSync_ = new message_filters::Synchronizer(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_); exactSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, boost::placeholders::_1, boost::placeholders::_2)); subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", getName().c_str(), diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index 1e8d12e9..7ff99925 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -100,13 +100,26 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); @@ -171,22 +184,22 @@ private: if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2)); - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2)); } @@ -195,11 +208,11 @@ private: image_transport::ImageTransport depth_it(depth_nh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(depth_nh, "camera_info", queueSize); - disparitySub_.subscribe(nh, "disparity/image", 1); - disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); + disparitySub_.subscribe(nh, "disparity/image", queueSize); + disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", queueSize); cloudPub_ = nh.advertise("cloud", 1); } diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index b57075ca..6ff125f1 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -108,13 +108,26 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); @@ -199,30 +212,30 @@ private: if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); if(approxSyncMaxInterval > 0.0) approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); if(approxSyncMaxInterval > 0.0) approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -235,9 +248,9 @@ private: image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); ros::NodeHandle left_nh(nh, "left"); ros::NodeHandle right_nh(nh, "right"); @@ -248,12 +261,12 @@ private: image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageDisparitySub_.subscribe(nh, "disparity", 1); + imageDisparitySub_.subscribe(nh, "disparity", queueSize); - imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); - imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + imageLeft_.subscribe(left_it, left_nh.resolveName("image"), queueSize, hintsLeft); + imageRight_.subscribe(right_it, right_nh.resolveName("image"), queueSize, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize); + cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize); } void depthCallback( diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 249015e4..87cd7fc7 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -92,9 +92,22 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approx = true; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_); @@ -115,7 +128,8 @@ private: ROS_INFO("Params:"); ROS_INFO(" approx=%s", approx?"true":"false"); - ROS_INFO(" queue_size=%d", queueSize); + ROS_INFO(" topic_queue_size=%d", queueSize); + ROS_INFO(" sync_queue_size=%d", syncQueueSize); ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str()); ROS_INFO(" wait_for_transform=%fs", waitForTransform_); ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); @@ -133,18 +147,18 @@ private: if(approx) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_); approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { fixedFrameId_.clear(); - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_); exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } - pointCloudSub_.subscribe(nh, "cloud", 1); - cameraInfoSub_.subscribe(nh, "camera_info", 1); + pointCloudSub_.subscribe(nh, "cloud", queueSize); + cameraInfoSub_.subscribe(nh, "camera_info", queueSize); } void callback( diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index fd17015f..6f6a3e17 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -73,14 +73,9 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; - bool approxSync = true; - pnh.param("queue_size", queueSize, queueSize); pnh.param("compress", compress_, compress_); pnh.param("uncompress", uncompress_, uncompress_); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); - rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this); rgbdImagePub_ = nh.advertise(nh.resolveName("rgbd_image") + "_relay", 1); } diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 4680a3d8..ff26c3d4 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -71,11 +71,6 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); - - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); - ros::NodeHandle rgb_nh(nh, nh.resolveName("rgbd_image") + "/rgb"); ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth"); image_transport::ImageTransport rgb_it(rgb_nh); diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 9e1a75de..d5746164 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -162,27 +162,27 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : if(subscribeInfoOnly) { ROS_INFO("subscribe_info_only=true"); - infoOnlyTopic_ = nh.subscribe("info", 1, &GuiWrapper::infoCallback, this); + infoOnlyTopic_ = nh.subscribe("info", this->getTopicQueueSize(), &GuiWrapper::infoCallback, this); } else { - infoTopic_.subscribe(nh, "info", 1); - mapDataTopic_.subscribe(nh, "mapData", 1); + infoTopic_.subscribe(nh, "info", this->getTopicQueueSize()); + mapDataTopic_.subscribe(nh, "mapData", this->getTopicQueueSize()); infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(this->getQueueSize()), + MyInfoMapSyncPolicy(this->getSyncQueueSize()), infoTopic_, mapDataTopic_); infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2)); } - goalTopic_.subscribe(nh, "goal_node", 1); - pathTopic_.subscribe(nh, "global_path", 1); + goalTopic_.subscribe(nh, "goal_node", this->getTopicQueueSize()); + pathTopic_.subscribe(nh, "global_path", this->getTopicQueueSize()); goalPathSync_ = new message_filters::Synchronizer( - MyGoalPathSyncPolicy(this->getQueueSize()), + MyGoalPathSyncPolicy(this->getSyncQueueSize()), goalTopic_, pathTopic_); goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this); + goalReachedTopic_ = nh.subscribe("goal_reached", this->getTopicQueueSize(), &GuiWrapper::goalReachedCallback, this); setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end } From fdd13c31f9574e72ed27a79321dfa00676176239 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Jun 2024 14:22:56 -0700 Subject: [PATCH 027/126] Make rtabmap_viz fails during cmake when rtabmap is not built with gui component --- rtabmap_viz/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 013895db..84ba092c 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_sync REQUIRED) find_package(tf2 REQUIRED) +find_package(RTABMap COMPONENTS gui REQUIRED) + include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include ) From 9a86ce9c906cacff2145804eace3f0b5063ae569 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 16:55:58 -0700 Subject: [PATCH 028/126] rtabmap.launch.py: added options to connect to nav2 (through goal_pose topic or action server). RVIZ/MapCloud: fixed downloading option. rtabmap node: all services are now in node namespace. Added dev containers for Humble and Jazzy. --- .devcontainer/{ => humble}/devcontainer.json | 0 .devcontainer/jazzy/devcontainer.json | 11 +++ rtabmap_launch/launch/rtabmap.launch.py | 7 +- .../rtabmap_rviz_plugins/MapCloudDisplay.h | 1 + rtabmap_rviz_plugins/src/MapCloudDisplay.cpp | 91 ++++++++++--------- rtabmap_slam/src/CoreWrapper.cpp | 76 ++++++++-------- rtabmap_viz/src/GuiWrapper.cpp | 20 ++-- 7 files changed, 113 insertions(+), 93 deletions(-) rename .devcontainer/{ => humble}/devcontainer.json (100%) create mode 100644 .devcontainer/jazzy/devcontainer.json diff --git a/.devcontainer/devcontainer.json b/.devcontainer/humble/devcontainer.json similarity index 100% rename from .devcontainer/devcontainer.json rename to .devcontainer/humble/devcontainer.json diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json new file mode 100644 index 00000000..f6f7e254 --- /dev/null +++ b/.devcontainer/jazzy/devcontainer.json @@ -0,0 +1,11 @@ +{ + "image": "introlab3it/rtabmap_ros:jazzy-latest", + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"] + } + }, + "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'" +} diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 4934bd7b..fda87a74 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -274,6 +274,7 @@ def launch_setup(context, *args, **kwargs): "odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context), "publish_tf": LaunchConfiguration('publish_tf_map'), "initial_pose": LaunchConfiguration('initial_pose'), + "use_action_for_goal": LaunchConfiguration('use_action_for_goal'), "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context), "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'), @@ -316,7 +317,8 @@ def launch_setup(context, *args, **kwargs): ("tag_detections", LaunchConfiguration('tag_topic')), ("fiducial_transforms", LaunchConfiguration('fiducial_topic')), ("odom", LaunchConfiguration('odom_topic')), - ("imu", LaunchConfiguration('imu_topic'))], + ("imu", LaunchConfiguration('imu_topic')), + ("goal_out", LaunchConfiguration('output_goal_topic'))], arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -425,6 +427,9 @@ def generate_launch_description(): DeclareLaunchArgument('output', default_value='screen', description='Control node output (screen or log).'), DeclareLaunchArgument('initial_pose', default_value='', description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'), + DeclareLaunchArgument('output_goal_topic', default_value='/goal_pose', description='Output goal topic (can be connected to nav2).'), + DeclareLaunchArgument('use_action_for_goal', default_value='false', description='Connect to nav2\'s navigate_to_pose action server instead of publishing the output goal topic.'), + DeclareLaunchArgument('ground_truth_frame_id', default_value='', description='e.g., "world"'), DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'), diff --git a/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h b/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h index d3f9f4a9..1a9ef89d 100644 --- a/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h +++ b/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h @@ -175,6 +175,7 @@ private: void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask ); private: + std::shared_ptr clientNode_; rclcpp::Publisher::SharedPtr republishNodeDataPub_; std::map cloud_infos_; diff --git a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp index 87dc50fa..61527917 100644 --- a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp +++ b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp @@ -57,7 +57,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include - namespace rtabmap_rviz_plugins { @@ -97,6 +96,10 @@ MapCloudDisplay::MapCloudDisplay() //QIcon icon; //this->setIcon(icon); + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"}); + clientNode_ = std::make_shared("_", options); + style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares", "Rendering mode to use, in order of computational complexity.", this, SLOT( updateStyle() ), this ); @@ -500,64 +503,64 @@ void MapCloudDisplay::updateCloudParameters() fromScan_ = cloud_from_scan_->getBool(); } -void MapCloudDisplay::downloadMap(bool /*graphOnly*/) +void MapCloudDisplay::downloadMap(bool graphOnly) { - RCLCPP_ERROR(rviz_ros_node_.lock()->get_raw_node()->get_logger(), "MapCloud plugin: DownloadMap still not working on ros2"); - return; - // FIXME: ros2: can connect to client, rtabmap returns data but the callback here is never called?! - /* auto request = std::make_shared(); request->global_map = false; request->optimized = true; request->graph_only = graphOnly; std::string rtabmapNs = download_namespace->getStdString(); - std::string srvName = uFormat("%s/get_map_data", rtabmapNs.c_str()); -// QMessageBox * messageBox = new QMessageBox( -// QMessageBox::NoIcon, -// tr("Calling \"%1\" service...").arg(srvName.c_str()), -// tr("Downloading the map... please wait (rviz could become gray!)"), -// QMessageBox::NoButton); -// messageBox->setAttribute(Qt::WA_DeleteOnClose, true); -// messageBox->show(); -// QApplication::processEvents(); -// uSleep(100); // hack make sure the text in the QMessageBox is shown... -// QApplication::processEvents(); + std::string srvName = rtabmapNs+"/get_map_data"; + QMessageBox * messageBox = new QMessageBox( + QMessageBox::NoIcon, + tr("Calling \"%1\" service...").arg(srvName.c_str()), + tr("Downloading the map... please wait (rviz could become gray!)"), + QMessageBox::NoButton, + getAssociatedWidget()); + messageBox->setAttribute(Qt::WA_DeleteOnClose, true); + messageBox->show(); + QApplication::processEvents(); + uSleep(100); // hack make sure the text in the QMessageBox is shown... + QApplication::processEvents(); - RVIZ_COMMON_LOG_WARNING(uFormat("Wait for service %s", srvName.c_str())); - auto client = rviz_ros_node_.lock()->get_raw_node()->create_client(srvName); + RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str())); + + auto client = clientNode_->create_client(srvName); if(client->wait_for_service(std::chrono::seconds(1))) { - using ServiceResponseFuture = rclcpp::Client::SharedFuture; - auto response_received_callback = [this, &graphOnly](ServiceResponseFuture future) { - auto result = future.get(); - RVIZ_COMMON_LOG_WARNING(uFormat("Process data")); + RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str())); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(clientNode_, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RVIZ_COMMON_LOG_INFO(uFormat("Process data")); + auto future = result.get(); if(graphOnly) { - //messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(result->data.graph.poses.size())); - //QApplication::processEvents(); - processMapData(result->data); - //messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(result->data.graph.poses.size())); - - // QTimer::singleShot(1000, messageBox, SLOT(close())); + messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size())); + QApplication::processEvents(); + processMapData(future->data); + messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size())); + QApplication::processEvents(); + QTimer::singleShot(1000, messageBox, SLOT(close())); } else { - //messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...") - // .arg(result->data.graph.poses.size()).arg(result->data.nodes.size())); - //QApplication::processEvents(); + messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...") + .arg(future->data.graph.poses.size()).arg(future->data.nodes.size())); + QApplication::processEvents(); this->reset(); - processMapData(result->data); - //messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!") - // .arg(result->data.graph.poses.size()).arg(result->data.nodes.size())); + processMapData(future->data); + messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!") + .arg(future->data.graph.poses.size()).arg(future->data.nodes.size())); - // QTimer::singleShot(1000, messageBox, SLOT(close())); + QTimer::singleShot(1000, messageBox, SLOT(close())); } - }; - RVIZ_COMMON_LOG_WARNING(uFormat("Calling service %s", srvName.c_str())); - auto result_future = client->async_send_request(request, response_received_callback); - RVIZ_COMMON_LOG_WARNING(uFormat("Wait")); - result_future.wait(); - RVIZ_COMMON_LOG_WARNING(uFormat("Wait end")); + } else { + std::string msg = uFormat("Failed to call service %s", srvName.c_str()); + RVIZ_COMMON_LOG_ERROR(msg); + messageBox->setText(msg.c_str()); + } } else { @@ -567,8 +570,8 @@ void MapCloudDisplay::downloadMap(bool /*graphOnly*/) srvName.c_str(), rtabmapNs.c_str()); RVIZ_COMMON_LOG_ERROR(msg); - //messageBox->setText(msg.c_str()); - }*/ + messageBox->setText(msg.c_str()); + } } void MapCloudDisplay::downloadNamespaceChanged() diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 1f2f94e3..c036cd68 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -208,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : if(useActionForGoal_) { RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support."); + useActionForGoal_ = false; } #endif useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_); @@ -647,45 +648,46 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } // setup services - updateSrv_ = this->create_service("update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resetSrv_ = this->create_service("reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pauseSrv_ = this->create_service("pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service("resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - loadDatabaseSrv_ = this->create_service("load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - triggerNewMapSrv_ = this->create_service("trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - backupDatabase_ = this->create_service("backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - detectMoreLoopClosuresSrv_ = this->create_service("detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - globalBundleAdjustmentSrv_ = this->create_service("global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cleanupLocalGridsSrv_ = this->create_service("cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeLocalizationSrv_ = this->create_service("set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeMappingSrv_ = this->create_service("set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodeDataSrv_ = this->create_service("get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapDataSrv_ = this->create_service("get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapData2Srv_ = this->create_service("get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapSrv_ = this->create_service("get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getProbMapSrv_ = this->create_service("get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - publishMapDataSrv_ = this->create_service("publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanSrv_ = this->create_service("get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanNodesSrv_ = this->create_service("get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setGoalSrv_ = this->create_service("set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cancelGoalSrv_ = this->create_service("cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLabelSrv_ = this->create_service("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - listLabelsSrv_ = this->create_service("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - removeLabelSrv_ = this->create_service("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - addLinkSrv_ = this->create_service("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodesInRadiusSrv_ = this->create_service("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + const std::string servicePrefix = get_name() + std::string("/"); + updateSrv_ = this->create_service(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + triggerNewMapSrv_ = this->create_service(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + backupDatabase_ = this->create_service(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + detectMoreLoopClosuresSrv_ = this->create_service(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + globalBundleAdjustmentSrv_ = this->create_service(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + cleanupLocalGridsSrv_ = this->create_service(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setModeLocalizationSrv_ = this->create_service(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setModeMappingSrv_ = this->create_service(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getNodeDataSrv_ = this->create_service(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getMapDataSrv_ = this->create_service(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getMapData2Srv_ = this->create_service(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getMapSrv_ = this->create_service(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getProbMapSrv_ = this->create_service(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + publishMapDataSrv_ = this->create_service(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getPlanNodesSrv_ = this->create_service(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setGoalSrv_ = this->create_service(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + getNodesInRadiusSrv_ = this->create_service(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = this->create_service("octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - octomapFullSrv_ = this->create_service("octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); #endif #endif //private services - setLogDebugSrv_ = this->create_service("log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogInfoSrv_ = this->create_service("log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogWarnSrv_ = this->create_service("log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogErrorSrv_ = this->create_service("log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); @@ -848,7 +850,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1)); #endif imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1)); - republishNodeDataSub_ = this->create_subscription("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1)); + republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1)); parametersClient_ = std::make_shared(this); auto on_parameter_event_callback = @@ -4417,14 +4419,12 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp) RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!"); } } + else #endif if(nextMetricGoalPub_->get_subscription_count()) { nextMetricGoalPub_->publish(poseMsg); - if(!useActionForGoal_) - { - lastPublishedMetricGoal_ = currentMetricGoal_; - } + lastPublishedMetricGoal_ = currentMetricGoal_; } } } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 5cc83933..8cb98327 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -142,7 +142,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : UEventsManager::addHandler(this); UEventsManager::addHandler(mainWindow_); - republishNodeDataPub_ = this->create_publisher("republish_node_data", 1); + republishNodeDataPub_ = this->create_publisher(rtabmapNodeName_+"/republish_node_data", 1); if(subscribeInfoOnly) { @@ -369,7 +369,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd(); if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory) { - if(!callEmptyService("reset")) + if(!callEmptyService(rtabmapNodeName_+"/reset")) { RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service"); } @@ -390,7 +390,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) callEmptyService("pause_odom"); // Pause rtabmap - if(!callEmptyService("pause")) + if(!callEmptyService(rtabmapNodeName_+"/pause")) { RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service"); } @@ -398,7 +398,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume) { // Resume rtabmap - if(!callEmptyService("resume")) + if(!callEmptyService(rtabmapNodeName_+"/resume")) { RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service"); } @@ -418,7 +418,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap) { - if(!callEmptyService("trigger_new_map")) + if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map")) { RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service"); } @@ -429,7 +429,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) UASSERT(cmdEvent->value2().isBool()); UASSERT(cmdEvent->value3().isBool()); - if(!callMapDataService("get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool())) + if(!callMapDataService(rtabmapNodeName_+"/get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool())) { this->post(new RtabmapEvent3DMap(1)); // service error } @@ -438,7 +438,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) { UASSERT(cmdEvent->value1().isStr() || cmdEvent->value1().isInt() || cmdEvent->value1().isUInt()); - auto client = this->create_client("set_goal"); + auto client = this->create_client(rtabmapNodeName_+"/set_goal"); if(client->wait_for_service(std::chrono::seconds(1))) { auto request = std::make_shared(); @@ -468,7 +468,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal) { - if(!callEmptyService("cancel_goal")) + if(!callEmptyService(rtabmapNodeName_+"/cancel_goal")) { RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service"); } @@ -478,7 +478,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) UASSERT(cmdEvent->value1().isStr()); UASSERT(cmdEvent->value2().isUndef() || cmdEvent->value2().isInt() || cmdEvent->value2().isUInt()); - auto client = this->create_client("set_label"); + auto client = this->create_client(rtabmapNodeName_+"/set_label"); if(client->wait_for_service(std::chrono::seconds(1))) { auto request = std::make_shared(); @@ -495,7 +495,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel) { UASSERT(cmdEvent->value1().isStr()); - auto client = this->create_client("remove_label"); + auto client = this->create_client(rtabmapNodeName_+"/remove_label"); if(client->wait_for_service(std::chrono::seconds(1))) { auto request = std::make_shared(); From eee311d4b674c162b5626357e11c239b01809647 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 17:01:19 -0700 Subject: [PATCH 029/126] ci: jazzy should be disabled --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 8b33446e..3bac8867 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -21,7 +21,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - ros_distro: [humble, iron, jazzy] + ros_distro: [humble, iron] include: - ros_distro: 'humble' ubuntu_distro: 'jammy' From 5d3b567fad7cf1d45759699bf8118b1af0bb8b44 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 18:42:20 -0700 Subject: [PATCH 030/126] Added turtlebot4 demo launch files --- .../launch/turtlebot4_ignition_demo.launch.py | 73 +++++++++++ .../launch/turtlebot4_slam.launch.py | 113 ++++++++++++++++++ 2 files changed, 186 insertions(+) create mode 100644 rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py create mode 100644 rtabmap_demos/launch/turtlebot4_slam.launch.py diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py new file mode 100644 index 00000000..c083d349 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -0,0 +1,73 @@ +# Example: +# 1) Launch simulator (turtlebot4, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py +# +# 2) Click on "Play" button on bottom-right of gazebo. +# +# 3) Click on double points ".." button on top right next to power button to undock. +# +# 4) Teleop the robot: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# 5) Send goals with RVIZ's "Nav2 Goal" button in action bar. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +ARGUMENTS = [ + DeclareLaunchArgument('rviz', default_value='true', + choices=['true', 'false'], description='Start rviz.'), + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('nav2', default_value='true', + choices=['true', 'false'], description='Start nav2.'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), +] + +def generate_launch_description(): + # Directories + pkg_turtlebot4_ignition_bringup = get_package_share_directory( + 'turtlebot4_ignition_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + # Paths + ignition_launch = PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot4_slam.launch.py']) + + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ignition_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ('slam', 'false'), + ('localization', 'false'), + ('nav2', LaunchConfiguration('nav2')), + ('rviz', LaunchConfiguration('rviz')) + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('qos', '2'), + ('use_sim_time', 'true') + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition + ld.add_action(ignition) + return ld \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4_slam.launch.py new file mode 100644 index 00000000..58dac4aa --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4_slam.launch.py @@ -0,0 +1,113 @@ +# Example with gazebo: +# 1) Launch simulator (turtlebot4 and nav2): +# $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true +# +# 2) Launch SLAM: +# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true qos:=2 +# OR +# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" qos:=2 use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true +# +# 3) Click on "Play" button on bottom-right of gazebo. +# +# 4) Click on double points ".." button on top right next to power button to undock. +# +# 5) Teleop the robot: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# 6) Send goals with RVIZ's "Nav2 Goal" button in action bar: + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + qos = LaunchConfiguration('qos') + localization = LaunchConfiguration('localization') + + icp_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'qos':qos + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'qos_scan':qos, + 'qos_image':qos, + 'qos_imu':qos, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false' + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'Mem/NotLinkedNodesKept':'false' + } + + remappings=[ + ('odom', 'icp_odom'), + ('rgb/image', '/oakd/rgb/preview/image_raw'), + ('rgb/camera_info', '/oakd/rgb/preview/camera_info'), + ('depth/image', '/oakd/rgb/preview/depth')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'qos', default_value='0', + description='QoS used for input sensor topics'), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[icp_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'icp_odometry:=warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) From c0d219cce4e5a55a8ae454ac49a6757fc895d5ff Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 18:44:26 -0700 Subject: [PATCH 031/126] fixed description typos --- rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py | 4 ++-- rtabmap_demos/launch/turtlebot4_slam.launch.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py index c083d349..fefb9cc9 100644 --- a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -2,9 +2,9 @@ # 1) Launch simulator (turtlebot4, nav2 and rtabmap): # $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py # -# 2) Click on "Play" button on bottom-right of gazebo. +# 2) Click on "Play" button on bottom-left of gazebo. # -# 3) Click on double points ".." button on top right next to power button to undock. +# 3) Click on double points ".." button on top-right next to power button to undock. # # 4) Teleop the robot: # $ ros2 run teleop_twist_keyboard teleop_twist_keyboard diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4_slam.launch.py index 58dac4aa..44769ffb 100644 --- a/rtabmap_demos/launch/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4_slam.launch.py @@ -7,9 +7,9 @@ # OR # $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" qos:=2 use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true # -# 3) Click on "Play" button on bottom-right of gazebo. +# 3) Click on "Play" button on bottom-left of gazebo. # -# 4) Click on double points ".." button on top right next to power button to undock. +# 4) Click on double points ".." button on top-right next to power button to undock. # # 5) Teleop the robot: # $ ros2 run teleop_twist_keyboard teleop_twist_keyboard From bed30814354cff821e69735bea0706ddc59aaf71 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 19:44:23 -0700 Subject: [PATCH 032/126] turtlebot4 demo: added odom_sensor_sync and added description to fix lidar/cloud misalignment. --- rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py | 4 ++++ rtabmap_demos/launch/turtlebot4_slam.launch.py | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py index fefb9cc9..d05f7666 100644 --- a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -1,3 +1,7 @@ +# +# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434, +# otherwise, the lidar and camera point cloud won't be aligned correctly. +# # Example: # 1) Launch simulator (turtlebot4, nav2 and rtabmap): # $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4_slam.launch.py index 44769ffb..49344e58 100644 --- a/rtabmap_demos/launch/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4_slam.launch.py @@ -1,3 +1,7 @@ +# +# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434, +# otherwise, the lidar and camera point cloud won't be aligned correctly. +# # Example with gazebo: # 1) Launch simulator (turtlebot4 and nav2): # $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true @@ -39,6 +43,7 @@ def generate_launch_description(): 'subscribe_rgbd':True, 'subscribe_scan':True, 'use_action_for_goal':True, + 'odom_sensor_sync': True, 'qos_scan':qos, 'qos_image':qos, 'qos_imu':qos, From 304636527e4b39a92f212b41da13bf322f8bc32d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Jun 2024 12:09:06 -0700 Subject: [PATCH 033/126] Updated turtlebot4 demo to be more robust to long corridors --- .../launch/turtlebot4_ignition_demo.launch.py | 9 ++++++--- rtabmap_demos/launch/turtlebot4_slam.launch.py | 12 ++++++++---- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py index d05f7666..e7294af6 100644 --- a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -10,10 +10,13 @@ # # 3) Click on double points ".." button on top-right next to power button to undock. # -# 4) Teleop the robot: -# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# 4) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2): +# $ ros2 launch explore_lite explore.launch.py # -# 5) Send goals with RVIZ's "Nav2 Goal" button in action bar. from ament_index_python.packages import get_package_share_directory diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4_slam.launch.py index 49344e58..45600927 100644 --- a/rtabmap_demos/launch/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4_slam.launch.py @@ -15,10 +15,13 @@ # # 4) Click on double points ".." button on top-right next to power button to undock. # -# 5) Teleop the robot: -# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2): +# $ ros2 launch explore_lite explore.launch.py # -# 6) Send goals with RVIZ's "Nav2 Goal" button in action bar: from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable @@ -58,7 +61,8 @@ def generate_launch_description(): # RTAB-Map's parameters should be strings: 'Reg/Strategy':'1', 'Reg/Force3DoF':'true', - 'Mem/NotLinkedNodesKept':'false' + 'Mem/NotLinkedNodesKept':'false', + 'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry } remappings=[ From 9c7927c4494bbe2d10fa1f7537301eecbfce4b11 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Jun 2024 20:13:58 +0000 Subject: [PATCH 034/126] devcontainer: update comment --- .devcontainer/devcontainer.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 36648e61..3f90ca0d 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,5 +7,5 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/catkin_ws", - "postAttachCommand": "apt remove -y ros-noetic-rtabmap-* && echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'" + "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'" } From 76c6a1fa8f8f5b657599297260c3ee124af4efde Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Jun 2024 20:31:23 +0000 Subject: [PATCH 035/126] examples: added missing exec_depend imu_complementary_filter for euroc_datasets.launch example (#1179) --- rtabmap_examples/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 42b080ff..47d37539 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -27,6 +27,7 @@ robot_localization imu_filter_madgwick + imu_complementary_filter realsense2_camera velodyne_pointcloud From 680fe729c5285b5a6aa6a1893f0073afe3d06267 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 1 Jul 2024 23:15:12 +0800 Subject: [PATCH 036/126] add validation check for scan-message (#1151) * add validation check for scan-message Signed-off-by: goes * remove abundant logger Signed-off-by: goes * fit into main Signed-off-by: GoesM --------- Signed-off-by: goes Signed-off-by: GoesM Co-authored-by: goes Co-authored-by: matlabbe --- rtabmap_conversions/src/MsgConversion.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 10250277..3903fc9c 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2591,6 +2591,24 @@ bool convertScanMsg( double waitForTransform, bool outputInFrameId) { + // scan message validation check + if(scan2dMsg.angle_increment == 0.0f) { + ROS_ERROR("convertScanMsg: angle_increment should not be 0!"); + return false; + } + if(scan2dMsg.range_min > scan2dMsg.range_max) { + ROS_ERROR("convertScanMsg: range_min (%f) should be smaller than range_max (%f)!", scan2dMsg.range_min, scan2dMsg.range_max); + return false; + } + if(scan2dMsg.angle_increment > 0 && scan2dMsg.angle_max < scan2dMsg.angle_min) { + ROS_ERROR("convertScanMsg: Angle increment (%f) should be negative if angle_min(%f) > angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + else if (scan2dMsg.angle_increment < 0 && scan2dMsg.angle_max > scan2dMsg.angle_min) { + ROS_ERROR("convertScanMsg: Angle increment (%f) should positive if angle_min(%f) < angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = getMovingTransform( scan2dMsg.header.frame_id, From a97efff760720132ced2a607f384dc5d28a0d296 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jul 2024 16:20:34 -0700 Subject: [PATCH 037/126] Fixed #1186 --- .devcontainer/devcontainer.json | 3 +- rtabmap_odom/src/OdometryROS.cpp | 221 +++++++++++++++---------------- 2 files changed, 111 insertions(+), 113 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 3f90ca0d..13ff5c22 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,5 +7,6 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/catkin_ws", - "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'" + "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'", + "runArgs": ["--privileged"] } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index b0277010..1219a87a 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -932,124 +932,121 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header } } - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) + if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers()) { - if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers()) + rtabmap_msgs::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers()); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + if(odomSensorDataPub_.getNumSubscribers()) { - rtabmap_msgs::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers()); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - if(odomSensorDataPub_.getNumSubscribers()) - { - odomSensorDataPub_.publish(msg); - } - if(odomSensorDataFeaturesPub_.getNumSubscribers()) - { - // remove data - msg.left = sensor_msgs::Image(); - msg.right = sensor_msgs::Image(); - msg.laser_scan = sensor_msgs::PointCloud2(); - msg.grid_ground.clear(); - msg.grid_obstacles.clear(); - msg.grid_empty_cells.clear(); - odomSensorDataFeaturesPub_.publish(msg); - } + odomSensorDataPub_.publish(msg); } - if(odomSensorDataCompressedPub_.getNumSubscribers()) + if(odomSensorDataFeaturesPub_.getNumSubscribers()) { - cv::Mat compressedImage; - cv::Mat compressedDepth; - cv::Mat compressedScan; - if(compressionParallelized_) - { - rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); - rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); - if(!data.imageRaw().empty()) - { - ctImage.start(); - } - if(!data.depthOrRightRaw().empty()) - { - ctDepth.start(); - } - if(!data.laserScanRaw().isEmpty()) - { - ctLaserScan.start(); - } - ctImage.join(); - ctDepth.join(); - ctLaserScan.join(); - - compressedImage = ctImage.getCompressedData(); - compressedDepth = ctDepth.getCompressedData(); - compressedScan = ctLaserScan.getCompressedData(); - } - else - { - compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); - compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - compressedScan = compressData2(data.laserScanRaw().data()); - } - if(!compressedImage.empty() && !data.stereoCameraModels().empty()) - { - data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); - } - else if(!compressedImage.empty() && !data.cameraModels().empty()) - { - data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); - } - if(!compressedScan.empty()) - { - data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? - LaserScan(compressedScan, - data.laserScanRaw().maxPoints(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().format(), - data.laserScanRaw().localTransform()): - LaserScan(compressedScan, - data.laserScanRaw().format(), - data.laserScanRaw().rangeMin(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().angleMin(), - data.laserScanRaw().angleMax(), - data.laserScanRaw().angleIncrement(), - data.laserScanRaw().localTransform()), false); - } - rtabmap_msgs::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - odomSensorDataCompressedPub_.publish(msg); + // remove data + msg.left = sensor_msgs::Image(); + msg.right = sensor_msgs::Image(); + msg.laser_scan = sensor_msgs::PointCloud2(); + msg.grid_ground.clear(); + msg.grid_obstacles.clear(); + msg.grid_empty_cells.clear(); + odomSensorDataFeaturesPub_.publish(msg); } - - if(visParams_) - { - if(icpParams_) - { - NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - else - { - NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - } - else // if(icpParams_) - { - NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - - statusDiagnostic_.setStatus(pose.isNull()); - if(syncDiagnostic_.get() && !pose.isNull()) - { - double curentRate = 1.0/(ros::WallTime::now()-time).toSec(); - syncDiagnostic_->tick(header.stamp, - maxUpdateRate_>0 ? maxUpdateRate_: - expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); - } - - previousStamp_ = header.stamp.toSec(); } + if(odomSensorDataCompressedPub_.getNumSubscribers()) + { + cv::Mat compressedImage; + cv::Mat compressedDepth; + cv::Mat compressedScan; + if(compressionParallelized_) + { + rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); + rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); + if(!data.imageRaw().empty()) + { + ctImage.start(); + } + if(!data.depthOrRightRaw().empty()) + { + ctDepth.start(); + } + if(!data.laserScanRaw().isEmpty()) + { + ctLaserScan.start(); + } + ctImage.join(); + ctDepth.join(); + ctLaserScan.join(); + + compressedImage = ctImage.getCompressedData(); + compressedDepth = ctDepth.getCompressedData(); + compressedScan = ctLaserScan.getCompressedData(); + } + else + { + compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); + compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + compressedScan = compressData2(data.laserScanRaw().data()); + } + if(!compressedImage.empty() && !data.stereoCameraModels().empty()) + { + data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); + } + else if(!compressedImage.empty() && !data.cameraModels().empty()) + { + data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); + } + if(!compressedScan.empty()) + { + data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? + LaserScan(compressedScan, + data.laserScanRaw().maxPoints(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().format(), + data.laserScanRaw().localTransform()): + LaserScan(compressedScan, + data.laserScanRaw().format(), + data.laserScanRaw().rangeMin(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().angleMin(), + data.laserScanRaw().angleMax(), + data.laserScanRaw().angleIncrement(), + data.laserScanRaw().localTransform()), false); + } + rtabmap_msgs::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + odomSensorDataCompressedPub_.publish(msg); + } + + if(visParams_) + { + if(icpParams_) + { + NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + else + { + NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + } + else // if(icpParams_) + { + NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + + statusDiagnostic_.setStatus(pose.isNull()); + if(syncDiagnostic_.get()) + { + double curentRate = 1.0/(ros::WallTime::now()-time).toSec(); + syncDiagnostic_->tick(header.stamp, + maxUpdateRate_>0 ? maxUpdateRate_: + expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: + previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); + } + + previousStamp_ = header.stamp.toSec(); } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) From 10ddeee4d01d9ecdf433c878b7a6b44c7e56c09a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 24 Aug 2024 14:03:49 -0700 Subject: [PATCH 038/126] Fixed #1201 --- rtabmap_util/src/nodelets/disparity_to_depth.cpp | 2 +- rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp | 2 +- rtabmap_util/src/nodelets/rgbd_split.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index 7beb7c8b..e7a11de5 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -45,7 +45,7 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : int qos = 0; qos = this->declare_parameter("qos", qos); - auto node = rclcpp::Node::make_shared(this->get_name()); + auto node = std::shared_ptr(this, [](auto *) {}); image_transport::ImageTransport it(node); pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 30bf9ce3..95548bf9 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -107,7 +107,7 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_); RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_); - auto node = rclcpp::Node::make_shared(this->get_name()); + auto node = std::shared_ptr(this, [](auto *) {}); image_transport::ImageTransport it(node); depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 9cae1eae..80af3118 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -46,7 +46,7 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - auto node = rclcpp::Node::make_shared(this->get_name()); + auto node = std::shared_ptr(this, [](auto *) {}); image_transport::ImageTransport it(node); rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); From 268609b0b70b97bb56c30985d397345c0dea753b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Sep 2024 15:47:59 -0700 Subject: [PATCH 039/126] Update README.md --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 87fa8918..b45ced39 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current Build Status - ROS 2 + ROS 2 Humble Build Status @@ -38,9 +38,13 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current Iron Build Status + + Jazzy + Build Status + Rolling - Build Status + Build Status Docker From 3eb0b47a55bd56ea6282e5fff157880dcdca753a Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 2 Sep 2024 12:38:25 +0800 Subject: [PATCH 040/126] Fixed node ID=0 issues as msgs may not have seq. (#1202) (cherry picked from commit 097cab06677e4618ef0e4132cc544bc319d1d9a6) --- rtabmap_conversions/src/MsgConversion.cpp | 2 ++ rtabmap_slam/src/CoreWrapper.cpp | 7 ++----- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 3903fc9c..5f59758e 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1402,6 +1402,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg) std::multimap words; std::vector wordsKpts; std::vector words3D; + cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors); if(msg.word_id_keys.size() != msg.word_id_values.size()) @@ -1456,6 +1457,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg) } s.setWords(words, wordsKpts, words3D, wordsDescriptors); s.sensorData() = sensorDataFromROS(msg.data); + s.sensorData().setId(msg.id); return s; } void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 7d21e5f1..08b56b4b 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1778,10 +1778,7 @@ void CoreWrapper::commonSensorDataCallback( } SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); - if(lastPoseIntermediate_) - { - data.setId(-1); - } + data.setId(lastPoseIntermediate_?-1:0); OdometryInfo odomInfo; if(odomInfoMsg.get()) @@ -2282,7 +2279,7 @@ void CoreWrapper::process( } // If not intermediate node - if(data.id() > 0) + if(data.id() >= 0) { localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_); tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)); From fc1387f7843aab6ee814bbea879a472d5f7c8e2c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 17:18:06 -0700 Subject: [PATCH 041/126] Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters) --- .../include/rtabmap_odom/OdometryROS.h | 14 ++- rtabmap_odom/src/OdometryROS.cpp | 110 ++++++++++++------ 2 files changed, 86 insertions(+), 38 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index d36a18d9..fe632fd4 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -55,7 +56,7 @@ class Odometry; namespace rtabmap_odom { -class OdometryROS : public nodelet::Nodelet +class OdometryROS : public nodelet::Nodelet, public UThread { public: @@ -94,6 +95,9 @@ private: virtual void onOdomInit() = 0; virtual void updateParameters(rtabmap::ParametersMap & parameters) {} + virtual void mainLoop(); + virtual void mainLoopKill(); + void callbackIMU(const sensor_msgs::ImuConstPtr& msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); @@ -138,6 +142,13 @@ private: tf::TransformListener tfListener_; ros::Subscriber imuSub_; + // Safe-threading + UMutex imuMutex_; + UMutex dataMutex_; + USemaphore dataReady_; + rtabmap::SensorData dataToProcess_; + std_msgs::Header dataHeaderToProcess_; + bool paused_; int resetCountdown_; int resetCurrentCount_; @@ -156,7 +167,6 @@ private: bool waitIMUToinit_; bool imuProcessed_; std::map imus_; - std::pair bufferedData_; rtabmap_util::ULogToRosout ulogToRosout_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 1219a87a..d9150749 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -93,6 +93,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : OdometryROS::~OdometryROS() { + this->join(true); delete odometry_; } @@ -379,6 +380,8 @@ void OdometryROS::onInit() NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str()); } + this->start(); + onOdomInit(); } @@ -433,17 +436,12 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); + UScopeMutex m(imuMutex_); + imus_.insert(std::make_pair(stamp, imu)); - - if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp()) - { - SensorData data = bufferedData_.first; - bufferedData_.first = SensorData(); - processData(data, bufferedData_.second); - } - if(imus_.size() > 1000) { + NODELET_WARN("Dropping imu data!"); imus_.erase(imus_.begin()); } } @@ -451,40 +449,75 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) void OdometryROS::processData(SensorData & data, const std_msgs::Header & header) { - if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); + if(dataMutex_.lockTry() == 0) { - NODELET_WARN("odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_.getTopic().c_str()); + dataToProcess_ = data; + dataHeaderToProcess_ = header; + dataReady_.release(); + dataMutex_.unlock(); + } + else + { + NODELET_INFO("Dropping image/scan data"); + } +} + +void OdometryROS::mainLoopKill() +{ + // in case we were waiting, unblock thread + dataReady_.release(); +} + +void OdometryROS::mainLoop() +{ + dataReady_.acquire(); + + if(!this->isRunning()) + { + // thread killed return; } - if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) - { - //NODELET_WARN("No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", stamp.toSec()); + UScopeMutex lock(dataMutex_); - // keep in cache to process later when we will receive imu msgs - if(bufferedData_.first.isValid()) + // aliases + SensorData & data = dataToProcess_; + std_msgs::Header & header = dataHeaderToProcess_; + + std::vector > imus; + { + UScopeMutex m(imuMutex_); + + if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) { - NODELET_ERROR("Overwriting previous data! Make sure IMU is " - "published faster than data rate. (last image stamp " - "buffered=%f and new one is %f, last imu stamp received=%f)", - bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + NODELET_WARN("odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_.getTopic().c_str()); + return; + } + + if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) + { + NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + return; + } + // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) + std::map::iterator iterEnd = imus_.lower_bound(header.stamp.toSec()); + if(iterEnd!= imus_.end()) + { + ++iterEnd; + } + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + { + imus.push_back(*iter); + imus_.erase(iter++); } - bufferedData_.first = data; - bufferedData_.second = header; - return; } - // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(header.stamp.toSec()); - if(iterEnd!= imus_.end()) + + for(size_t i=0; i::iterator iter=imus_.begin(); iter!=iterEnd;) - { - //NODELET_WARN("img callback: process imu %f", iter->first); - SensorData dataIMU(iter->second, 0, iter->first); + SensorData dataIMU(imus[i].second, 0, imus[i].first); odometry_->process(dataIMU); - imus_.erase(iter++); imuProcessed_ = true; } @@ -1020,20 +1053,21 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header odomSensorDataCompressedPub_.publish(msg); } + double delay = (ros::Time::now() - header.stamp).toSec(); if(visParams_) { if(icpParams_) { - NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec(), delay); } else { - NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec(), delay); } } else // if(icpParams_) { - NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec(), delay); } statusDiagnostic_.setStatus(pose.isNull()); @@ -1066,14 +1100,18 @@ bool OdometryROS::resetToPose(rtabmap_msgs::ResetPose::Request& req, rtabmap_msg void OdometryROS::reset(const Transform & pose) { + UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; - bufferedData_.first= SensorData(); + dataToProcess_ = SensorData(); + dataHeaderToProcess_ = std_msgs::Header(); + imuMutex_.lock(); imus_.clear(); + imuMutex_.unlock(); this->flushCallbacks(); } From 1e7df7e276975819934760ee5538c1f9e8224894 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 17:23:11 -0700 Subject: [PATCH 042/126] Changed a log from info->debug --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index d9150749..133de117 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -457,10 +457,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header dataReady_.release(); dataMutex_.unlock(); } - else - { - NODELET_INFO("Dropping image/scan data"); - } + //else + //{ + // NODELET_INFO("Dropping image/scan data"); + //} } void OdometryROS::mainLoopKill() From 76f1c50d19d7ba6ccb35abba402238a47a312a6a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 18:02:54 -0700 Subject: [PATCH 043/126] Changed a log from info->debug --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 133de117..a46d4fb2 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -457,10 +457,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header dataReady_.release(); dataMutex_.unlock(); } - //else - //{ - // NODELET_INFO("Dropping image/scan data"); - //} + else + { + NODELET_DEBUG("Dropping image/scan data"); + } } void OdometryROS::mainLoopKill() From c956e3780fa2e2e1639ab583b7fe5dcaec644c50 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 4 Sep 2024 00:46:44 -0700 Subject: [PATCH 044/126] Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206) * Fixed odometry latency * Fixed node ID=0 issues as msgs may not have seq. (#1202) (cherry picked from commit 097cab06677e4618ef0e4132cc544bc319d1d9a6) * Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters) * Changed a log from info->debug * Changed a log from info->debug * Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes. * fixed merge * Include everything needed in example launch files for simple launch. Small fixes. --------- Co-authored-by: Borong Yuan --- rtabmap_conversions/src/MsgConversion.cpp | 6 - rtabmap_examples/launch/depthai.launch.py | 72 ++++++++++ .../launch/realsense_d400.launch.py | 27 ++-- .../launch/realsense_d435i_color.launch.py | 50 +++---- .../launch/realsense_d435i_infra.launch.py | 30 ++++- .../launch/realsense_d435i_stereo.launch.py | 30 ++++- rtabmap_examples/launch/vlp16.launch.py | 35 ++--- rtabmap_examples/launch/zed.launch.py | 76 +++++++++++ rtabmap_launch/launch/rtabmap.launch.py | 18 ++- .../include/rtabmap_odom/OdometryROS.h | 17 ++- rtabmap_odom/src/ICPOdometryNode.cpp | 5 +- rtabmap_odom/src/OdometryROS.cpp | 126 ++++++++++++------ rtabmap_odom/src/RGBDOdometryNode.cpp | 5 +- rtabmap_odom/src/StereoOdometryNode.cpp | 5 +- rtabmap_odom/src/nodelets/icp_odometry.cpp | 7 +- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 33 +++-- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 35 ++--- rtabmap_slam/src/CoreWrapper.cpp | 3 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 10 +- rtabmap_sync/src/nodelets/rgb_sync.cpp | 9 +- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 9 +- rtabmap_sync/src/nodelets/rgbdx_sync.cpp | 9 +- rtabmap_sync/src/nodelets/stereo_sync.cpp | 9 +- .../src/nodelets/disparity_to_depth.cpp | 6 +- .../src/nodelets/pointcloud_to_depthimage.cpp | 6 +- rtabmap_util/src/nodelets/rgbd_split.cpp | 6 +- rtabmap_viz/src/GuiWrapper.cpp | 1 - 27 files changed, 465 insertions(+), 180 deletions(-) create mode 100644 rtabmap_examples/launch/depthai.launch.py create mode 100644 rtabmap_examples/launch/zed.launch.py diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 25294bd7..0d820fb4 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1988,12 +1988,6 @@ rtabmap::Transform getTransform( { // TF ready? rtabmap::Transform transform; - std::string errString; - if(!tfBuffer.canTransform(fromFrameId, toFrameId, tf2_ros::fromMsg(stamp), tf2::durationFromSec(waitForTransform), &errString)) - { - UWARN("(can transform %s -> %s?) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), errString.c_str(), waitForTransform); - return rtabmap::Transform(); - } try { geometry_msgs::msg::TransformStamped tmp; diff --git a/rtabmap_examples/launch/depthai.launch.py b/rtabmap_examples/launch/depthai.launch.py new file mode 100644 index 00000000..8918bba0 --- /dev/null +++ b/rtabmap_examples/launch/depthai.launch.py @@ -0,0 +1,72 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros) +# Example: +# $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters=[{'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'depth_aligned': 'false', + 'enableRviz': 'false', + 'monoResolution': '400p'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=parameters, + remappings=[('rgb/image', '/right/image_rect'), + ('rgb/camera_info', '/right/camera_info'), + ('depth/image', '/stereo/depth')]), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ]) diff --git a/rtabmap_examples/launch/realsense_d400.launch.py b/rtabmap_examples/launch/realsense_d400.launch.py index 79c1ffac..d2e47aff 100644 --- a/rtabmap_examples/launch/realsense_d400.launch.py +++ b/rtabmap_examples/launch/realsense_d400.launch.py @@ -2,16 +2,16 @@ # A realsense D400 series # Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238) # Example: -# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true -# # $ ros2 launch rtabmap_examples realsense_d400.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false + +import os + +from ament_index_python.packages import get_package_share_directory 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 Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -27,7 +27,18 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'align_depth.enable': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index 4839874b..298b800f 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -6,10 +6,14 @@ # # $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + 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 Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -23,11 +27,26 @@ def generate_launch_description(): ('imu', '/imu/data'), ('rgb/image', '/camera/color/image_raw'), ('rgb/camera_info', '/camera/color/camera_info'), - ('depth/image', '/camera/realigned_depth_to_color/image_raw')] + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] return LaunchDescription([ - # Nodes to launch + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'align_depth.enable': 'true', + 'enable_sync': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, @@ -43,26 +62,7 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=parameters, remappings=remappings), - - # Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564 - # Generate point cloud from not aligned depth - Node( - package='rtabmap_util', executable='point_cloud_xyz', output='screen', - parameters=[{'approx_sync':False}], - remappings=[('depth/image', '/camera/depth/image_rect_raw'), - ('depth/camera_info', '/camera/depth/camera_info'), - ('cloud', '/camera/cloud_from_depth')]), - - # Generate aligned depth to color camera from the point cloud above - Node( - package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen', - parameters=[{ 'decimation':2, - 'fixed_frame_id':'camera_link', - 'fill_holes_size':1}], - remappings=[('camera_info', '/camera/color/camera_info'), - ('cloud', '/camera/cloud_from_depth'), - ('image_raw', '/camera/realigned_depth_to_color/image_raw')]), - + # Compute quaternion of the IMU Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index d4a2116f..140e995b 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -2,15 +2,16 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + 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 Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -28,7 +29,22 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index 43f93a61..ce879554 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -2,15 +2,16 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + 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 Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -28,7 +29,22 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py index dfc6927d..84bdf92e 100644 --- a/rtabmap_examples/launch/vlp16.launch.py +++ b/rtabmap_examples/launch/vlp16.launch.py @@ -1,15 +1,16 @@ # Example: -# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py -# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py -# -# SLAM: # $ ros2 launch rtabmap_examples vlp16.launch.py +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): @@ -28,6 +29,17 @@ def generate_launch_description(): description='Enable lidar deskewing'), # Nodes to launch + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_driver'), 'launch'), + '/velodyne_driver_node-VLP16-launch.py']), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_pointcloud'), 'launch'), + '/velodyne_transform_node-VLP16-launch.py']), + ), + Node( package='rtabmap_odom', executable='icp_odometry', output='screen', parameters=[{ @@ -57,18 +69,7 @@ def generate_launch_description(): remappings=[ ('scan_cloud', '/velodyne_points') ]), - - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{ - 'max_clouds':10, - 'fixed_frame_id':'', - 'use_sim_time':use_sim_time, - }], - remappings=[ - ('cloud', 'odom_filtered_input_scan') - ]), - + Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[{ @@ -102,7 +103,7 @@ def generate_launch_description(): 'Icp/CorrespondenceRatio': '0.2' }], remappings=[ - ('scan_cloud', 'assembled_cloud') + ('scan_cloud', 'odom_filtered_input_scan') ], arguments=[ '-d' # This will delete the previous database (~/.ros/rtabmap.db) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py new file mode 100644 index 00000000..0b89cec5 --- /dev/null +++ b/rtabmap_examples/launch/zed.launch.py @@ -0,0 +1,76 @@ +# Requirements: +# A ZED camera +# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper) +# Example: +# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def generate_launch_description(): + + # Hack to override grab_resolution parameter without changing any files + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'") + + camera_model = LaunchConfiguration('camera_model'), + + parameters=[{'frame_id':'zed_camera_link', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', '/zed/zed_node/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': camera_model, + 'ros_params_override_path': zed_override_file.name}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=parameters, + remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ]) diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index fda87a74..64e70aff 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_sync', executable='rgbd_sync', name="rgbd_sync", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])), parameters=[{ "approx_sync": LaunchConfiguration('approx_rgbd_sync'), @@ -120,6 +121,7 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])), parameters=[{ "approx_sync": LaunchConfiguration('approx_rgbd_sync'), @@ -139,6 +141,7 @@ def launch_setup(context, *args, **kwargs): # Relay rgbd_image Node( package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])), parameters=[{ "qos": LaunchConfiguration('qos_image')}], @@ -148,6 +151,7 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay_uncompress", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])), parameters=[{ "uncompress": True, @@ -160,6 +164,7 @@ def launch_setup(context, *args, **kwargs): # RGB-D odometry Node( package='rtabmap_odom', executable='rgbd_odometry', name="rgbd_odometry", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -195,6 +200,7 @@ def launch_setup(context, *args, **kwargs): # Stereo odometry Node( package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -231,6 +237,7 @@ def launch_setup(context, *args, **kwargs): # ICP odometry Node( package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + emulate_tty=True, condition=IfCondition(LaunchConfiguration('icp_odometry')), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -260,6 +267,7 @@ def launch_setup(context, *args, **kwargs): Node( package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen", + emulate_tty=True, parameters=[{ "subscribe_depth": LaunchConfiguration('depth'), "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), @@ -325,6 +333,7 @@ def launch_setup(context, *args, **kwargs): Node( package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output='screen', + emulate_tty=True, parameters=[{ "subscribe_depth": LaunchConfiguration('depth'), "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), @@ -368,12 +377,15 @@ def launch_setup(context, *args, **kwargs): arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), Node( package='rtabmap_util', executable='point_cloud_xyzrgb', name="point_cloud_xyzrgb", output='screen', + emulate_tty=True, condition=IfCondition(LaunchConfiguration("rviz")), parameters=[{ "decimation": 4, "voxel_size": 0.0, "approx_sync": LaunchConfiguration('approx_sync'), - "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval') + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info') }], remappings=[ ('left/image', LaunchConfiguration('left_image_topic_relay')), @@ -418,8 +430,8 @@ def generate_launch_description(): DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'), DeclareLaunchArgument('namespace', default_value='rtabmap', description=''), DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'), - DeclareLaunchArgument('topic_queue_size', default_value='1', description='Queue size of individual topic subscribers.'), - DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'), + DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'), + DeclareLaunchArgument('queue_size', default_value='2', description='Backward compatibility, use "sync_queue_size" instead.'), DeclareLaunchArgument('qos', default_value='1', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''), DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'), diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index ae2f7d5f..b42ebbe0 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -59,7 +60,7 @@ class Odometry; namespace rtabmap_odom { -class OdometryROS : public rclcpp::Node +class OdometryROS : public rclcpp::Node, public UThread { public: @@ -98,12 +99,17 @@ protected: private: + virtual void mainLoop(); + virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} virtual void onOdomInit() {} void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); +protected: + rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_; + private: rtabmap::Odometry * odometry_; @@ -147,6 +153,14 @@ private: std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; rclcpp::Subscription::SharedPtr imuSub_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + + // Safe-threading + UMutex imuMutex_; + UMutex dataMutex_; + USemaphore dataReady_; + rtabmap::SensorData dataToProcess_; + std_msgs::msg::Header dataHeaderToProcess_; bool paused_; int resetCountdown_; @@ -166,7 +180,6 @@ private: bool waitIMUToinit_; bool imuProcessed_; std::map imus_; - std::pair bufferedData_; std::string configPath_; rtabmap::Transform initialPose_; diff --git a/rtabmap_odom/src/ICPOdometryNode.cpp b/rtabmap_odom/src/ICPOdometryNode.cpp index da85a658..f83264d0 100644 --- a/rtabmap_odom/src/ICPOdometryNode.cpp +++ b/rtabmap_odom/src/ICPOdometryNode.cpp @@ -70,7 +70,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 8605cf38..caa436a4 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -97,6 +97,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o initialPose_(Transform::getIdentity()), ulogToRosout_(this) { + dataCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + int qos = this->declare_parameter("qos", (int)qos_); qos_ = (rmw_qos_reliability_policy_t)qos; @@ -204,6 +206,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o OdometryROS::~OdometryROS() { + this->join(true); delete odometry_; } @@ -365,14 +368,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams) Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); if(waitIMUToinit_) { - int queueSize = 10; - this->get_parameter_or("queue_size", queueSize, queueSize); + imuCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = imuCallbackGroup_; + int queueSize = this->declare_parameter("imu_queue_size", 200); int qosImu = this->declare_parameter("qos_imu", (int)qos_); - imuSub_ = create_subscription("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1)); + imuSub_ = create_subscription("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1), options); RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name()); RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu); + RCLCPP_INFO(this->get_logger(), "odometry: imu_queue_size = %d", queueSize); } + this->start(); + onOdomInit(); } @@ -408,6 +416,8 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) if(!this->isPaused()) { double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp); + //RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds()); + rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); if(this->frameId().compare(msg->header.frame_id) != 0) { @@ -428,18 +438,12 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); + UScopeMutex m(imuMutex_); imus_.insert(std::make_pair(stamp, imu)); - //RCLCPP_WARN(get_logger(), "Received imu: %f", stamp); - - if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp()) - { - SensorData data = bufferedData_.first; - bufferedData_.first = SensorData(); - processData(data, bufferedData_.second); - } if(imus_.size() > 1000) { + RCLCPP_WARN(this->get_logger(), "Dropping imu data!"); imus_.erase(imus_.begin()); } } @@ -447,45 +451,78 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { - if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + if(dataMutex_.lockTry() == 0) { - RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); + dataToProcess_ = data; + dataHeaderToProcess_ = header; + dataReady_.release(); + dataMutex_.unlock(); + } + else + { + RCLCPP_DEBUG(get_logger(), "Dropping image/scan data"); + } +} + +void OdometryROS::mainLoopKill() +{ + // in case we were waiting, unblock thread + dataReady_.release(); +} + +void OdometryROS::mainLoop() +{ + dataReady_.acquire(); + + if(!this->isRunning()) + { + // thread killed return; } - if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) - { - //RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp)); + UScopeMutex lock(dataMutex_); - // keep in cache to process later when we will receive imu msgs - if(bufferedData_.first.isValid()) + // aliases + SensorData & data = dataToProcess_; + std_msgs::msg::Header & header = dataHeaderToProcess_; + + std::vector > imus; + { + UScopeMutex m(imuMutex_); + + if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) { - RCLCPP_ERROR(this->get_logger(), "Overwriting previous data! Make sure IMU is " - "published faster than data rate. (last image stamp " - "buffered=%f and new one is %f, last imu stamp received=%f)", - bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); + return; } - bufferedData_.first = data; - bufferedData_.second = header; - return; - } - // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); - if(iterEnd!= imus_.end()) + + if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) + { + RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + return; + } + // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) + std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + if(iterEnd!= imus_.end()) + { + ++iterEnd; + } + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + { + imus.push_back(*iter); + imus_.erase(iter++); + } + } // end imu lock + + for(size_t i=0; i::iterator iter=imus_.begin(); iter!=iterEnd;) - { - //NODELET_WARN("img callback: process imu %f", iter->first); - SensorData dataIMU(iter->second, 0, iter->first); + SensorData dataIMU(imus[i].second, 0, imus[i].first); odometry_->process(dataIMU); - imus_.erase(iter++); imuProcessed_ = true; } - //RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp)); - Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { @@ -1017,20 +1054,21 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h odomSensorDataCompressedPub_->publish(msg); } + double delay = (now()-header.stamp).seconds(); if(visParams_) { if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } else { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } } else // if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } statusDiagnostic_.setStatus(pose.isNull()); @@ -1067,14 +1105,18 @@ void OdometryROS::resetToPose( void OdometryROS::reset(const Transform & pose) { + UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; - bufferedData_.first= SensorData(); + dataToProcess_ = SensorData(); + dataHeaderToProcess_ = std_msgs::msg::Header(); + imuMutex_.lock(); imus_.clear(); + imuMutex_.unlock(); this->flushCallbacks(); } diff --git a/rtabmap_odom/src/RGBDOdometryNode.cpp b/rtabmap_odom/src/RGBDOdometryNode.cpp index ad203b99..270d9ede 100644 --- a/rtabmap_odom/src/RGBDOdometryNode.cpp +++ b/rtabmap_odom/src/RGBDOdometryNode.cpp @@ -76,7 +76,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/StereoOdometryNode.cpp b/rtabmap_odom/src/StereoOdometryNode.cpp index d8976b7b..60a729ce 100644 --- a/rtabmap_odom/src/StereoOdometryNode.cpp +++ b/rtabmap_odom/src/StereoOdometryNode.cpp @@ -76,7 +76,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 4babddd3..4476870c 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); - scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1)); - cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1)); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + + scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 4a51d929..697da963 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -63,8 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : exactSync5_(0), approxSync6_(0), exactSync6_(0), - topicQueueSize_(1), - syncQueueSize_(5), + topicQueueSize_(10), + syncQueueSize_(2), keepColor_(false) { OdometryROS::init(false, true, false); @@ -125,29 +125,32 @@ void RGBDOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + std::string subscribedTopic; std::string subscribedTopicsMsg; if(subscribeRGBD) { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras == 2) @@ -327,7 +330,7 @@ void RGBDOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -336,7 +339,7 @@ void RGBDOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -348,9 +351,9 @@ 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()); - image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + 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); if(approxSync) { @@ -366,10 +369,12 @@ void RGBDOdometry::onOdomInit() } subscribedTopic = image_mono_sub_.getSubscriber().getTopic(); - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + topicQueueSize_, + syncQueueSize_, image_mono_sub_.getSubscriber().getTopic().c_str(), image_depth_sub_.getSubscriber().getTopic().c_str(), info_sub_.getSubscriber()->get_topic_name()); diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 7a291ec4..c6517eba 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -63,8 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : exactSync5_(0), approxSync6_(0), exactSync6_(0), - topicQueueSize_(1), - syncQueueSize_(5), + topicQueueSize_(10), + syncQueueSize_(2), keepColor_(false) { OdometryROS::init(true, true, false); @@ -120,29 +120,32 @@ void StereoOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false"); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + std::string subscribedTopic; std::string subscribedTopicsMsg; if(subscribeRGBD) { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras == 2) @@ -323,7 +326,7 @@ void StereoOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = @@ -333,7 +336,7 @@ void StereoOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -345,10 +348,10 @@ void StereoOdometry::onOdomInit() else { image_transport::TransportHints hints(this); - imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); if(approxSync) { @@ -364,10 +367,12 @@ void StereoOdometry::onOdomInit() } subscribedTopic = imageRectLeft_.getTopic(); - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + topicQueueSize_, + syncQueueSize_, imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getSubscriber()->get_topic_name(), diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index c036cd68..e3cd3548 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2316,7 +2316,7 @@ void CoreWrapper::process( { timeRtabmap = timer.ticks(); } - RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", + RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs delay=%.4fs (local map=%d, WM=%d)", rtabmap_.getLastLocationId(), rate_>0?1.0f/rate_:0, rtabmap_.getTimeThreshold()/1000.0f, @@ -2324,6 +2324,7 @@ void CoreWrapper::process( timeRtabmap, timeUpdateMaps, timePublishMaps, + (now() - lastPoseStamp_).seconds(), (int)rtabmap_.getLocalOptimizedPoses().size(), rtabmap_.getWMSize()+rtabmap_.getSTMSize()); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0)); diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 49fd026d..746510a5 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -31,8 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : - topicQueueSize_(1), - syncQueueSize_(10), + topicQueueSize_(10), + syncQueueSize_(2), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -699,8 +699,12 @@ void CommonDataSubscriber::setupCallbacks( uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " "header are set. If topics are coming from different computers, make sure " - "the clocks of the computers are synchronized (\"ntpdate\"). %s%s", + "the clocks of the computers are synchronized (\"ntpdate\"). Ajusting " + "topic_queue_size (%d) and sync_queue_size (%d) can also help for better " + "synchronization if framerates and/or delays are different. %s%s", name_.c_str(), + topicQueueSize_, + syncQueueSize_, approxSync_? uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str(): "Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.", diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 9c0cf0be..eaafadf3 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -51,8 +51,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; int qos = 0; double approxSyncMaxInterval = 0.0; @@ -115,8 +115,11 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", this->get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 0104b169..f8b05495 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -53,8 +53,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : approxSyncDepth_(0), exactSyncDepth_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; double approxSyncMaxInterval = 0.0; int qos = 0; @@ -128,8 +128,11 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index c78d6d58..c5d150e0 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -42,8 +42,8 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : SYNC_INIT(rgbd7), SYNC_INIT(rgbd8) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; @@ -151,8 +151,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init("", uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index eed21804..1978731d 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -50,8 +50,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = false; double approxSyncMaxInterval = 0.0; int qos = 0; @@ -117,8 +117,11 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index e7a11de5..be2448af 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -45,10 +45,8 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : int qos = 0; qos = this->declare_parameter("qos", qos); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); sub_ = create_subscription("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); } diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 95548bf9..0443c0b6 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -107,10 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_); RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm - depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters + depthImage16Pub_ = image_transport::create_camera_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm + depthImage32Pub_ = image_transport::create_camera_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters pointCloudTransformedPub_ = create_publisher("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cameraInfo16Pub_ = create_publisher(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); cameraInfo32Pub_ = create_publisher(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 80af3118..c0a9fc2b 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -46,10 +46,8 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 8cb98327..0f8cb410 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -335,7 +335,6 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters(); std::vector rosParameters; - auto node = rclcpp::Node::make_shared("rtabmap_viz"); for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) { //save only parameters with valid names From dac04faef0d1cec6ad907916e4c87bb4d91818b5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 4 Sep 2024 19:38:05 -0700 Subject: [PATCH 045/126] zed example: added use_zed_odometry option --- rtabmap_examples/launch/zed.launch.py | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index 0b89cec5..9344867e 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -9,10 +9,12 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import UnlessCondition import tempfile @@ -26,25 +28,36 @@ def generate_launch_description(): " general:\n"+ " grab_resolution: 'VGA'") - camera_model = LaunchConfiguration('camera_model'), + camera_model = LaunchConfiguration('camera_model') + use_zed_odometry = LaunchConfiguration('use_zed_odometry') parameters=[{'frame_id':'zed_camera_link', 'subscribe_rgbd':True, - 'subscribe_odom_info':True, + 'subscribe_odom_info': not use_zed_odometry, 'approx_sync':False, 'wait_imu_to_init':True}] remappings=[('imu', '/zed/zed_node/imu/data')] + if use_zed_odometry: + remappings.append(('odom', '/zed/zed_node/odom')) + return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'use_zed_odometry', default_value='false', + description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + # Launch camera driver IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('zed_wrapper'), 'launch'), '/zed_camera.launch.py']), launch_arguments={'camera_model': camera_model, - 'ros_params_override_path': zed_override_file.name}.items(), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': use_zed_odometry, + 'publish_map_tf': 'false'}.items(), ), # Sync right/depth/camera_info together @@ -58,8 +71,9 @@ def generate_launch_description(): # Visual odometry Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', + condition=UnlessCondition(use_zed_odometry), parameters=parameters, - remappings=remappings), + remappings=remappings,), # VSLAM Node( From 18f2788a375ceb7f2827632831a23afa30b5310a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 5 Sep 2024 21:05:50 -0700 Subject: [PATCH 046/126] zed.launch.py: Fixed use_zed_odometry arg behavior --- rtabmap_examples/launch/zed.launch.py | 49 +++++++++++++++------------ 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index 9344867e..ccefaee7 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -8,17 +8,20 @@ import os from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription +from launch import LaunchDescription, LaunchContext from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import UnlessCondition import tempfile -def generate_launch_description(): +parameters = [] +remappings = [] + +def launch_setup(context: LaunchContext, *args, **kwargs): # Hack to override grab_resolution parameter without changing any files with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: @@ -28,36 +31,28 @@ def generate_launch_description(): " general:\n"+ " grab_resolution: 'VGA'") - camera_model = LaunchConfiguration('camera_model') - use_zed_odometry = LaunchConfiguration('use_zed_odometry') - parameters=[{'frame_id':'zed_camera_link', 'subscribe_rgbd':True, - 'subscribe_odom_info': not use_zed_odometry, 'approx_sync':False, 'wait_imu_to_init':True}] remappings=[('imu', '/zed/zed_node/imu/data')] - if use_zed_odometry: + if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]: remappings.append(('odom', '/zed/zed_node/odom')) - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_zed_odometry', default_value='false', - description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), - + else: + parameters.append({'subscribe_odom_info': True}) + + return [ # Launch camera driver IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('zed_wrapper'), 'launch'), '/zed_camera.launch.py']), - launch_arguments={'camera_model': camera_model, - 'ros_params_override_path': zed_override_file.name, - 'publish_tf': use_zed_odometry, - 'publish_map_tf': 'false'}.items(), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': LaunchConfiguration('use_zed_odometry'), + 'publish_map_tf': 'false'}.items(), ), # Sync right/depth/camera_info together @@ -71,7 +66,7 @@ def generate_launch_description(): # Visual odometry Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', - condition=UnlessCondition(use_zed_odometry), + condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')), parameters=parameters, remappings=remappings,), @@ -87,4 +82,16 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=parameters, remappings=remappings) + ] + + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_zed_odometry', default_value='false', + description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + + OpaqueFunction(function=launch_setup) ]) From 9deb8a2a81f7a12c41d2ec61868d0ab24b5e5510 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 5 Sep 2024 22:04:20 -0700 Subject: [PATCH 047/126] Fixed #1198 --- .../rtabmap_util/obstacles_detection.hpp | 2 + .../src/nodelets/obstacles_detection.cpp | 47 ++++++++++++++++++- 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp index 6cadfc1a..9610886a 100644 --- a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp +++ b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp @@ -56,6 +56,8 @@ private: rtabmap::LocalGridMaker localMapMaker_; bool mapFrameProjection_; bool warned_; + float rangeMin_; + float rangeMax_; std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index ac535b7b..c9a99db0 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -45,7 +45,9 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : frameId_("base_link"), waitForTransform_(0.2), mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()), - warned_(false) + warned_(false), + rangeMin_(0), + rangeMax_(0) { ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); @@ -75,6 +77,8 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : } localMapMaker_.parseParameters(gridParameters); + rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMin(), rangeMin_); + rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMax(), rangeMax_); tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock()); tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_); @@ -86,6 +90,42 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : cloudSub_ = create_subscription("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1)); } +pcl::PointCloud rangeFiltering( + const pcl::PointCloud & cloud, + float rangeMin, + float rangeMax) +{ + if(!cloud.empty() && (rangeMin > 0.0f || rangeMax > 0.0f)) + { + pcl::PointCloud output; + output.reserve(cloud.size()); + int oi = 0; + float rangeMinSqrd = rangeMin * rangeMin; + float rangeMaxSqrd = rangeMax * rangeMax; + for(size_t i=0; i 0.0f && r < rangeMinSqrd) + { + continue; + } + if(rangeMax > 0.0f && r > rangeMaxSqrd) + { + continue; + } + + output.push_back(pt); + ++oi; + } + output.resize(oi); + return output; + } + + return cloud; +} + void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { rclcpp::Time time = now(); @@ -137,6 +177,11 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar inputCloud->is_dense = true; } + if(rangeMin_ > 0.0f || rangeMax_ > 0.0f) + { + *inputCloud = rangeFiltering(*inputCloud, rangeMin_, rangeMax_); + } + //Common variables for all strategies pcl::IndicesPtr ground, obstacles; pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); From 28f997477aa80130da4c0915d4f0182280c1a9a7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Sep 2024 23:26:06 -0700 Subject: [PATCH 048/126] Fixed #1207 --- rtabmap_slam/include/rtabmap_slam/CoreWrapper.h | 3 ++- rtabmap_slam/src/CoreWrapper.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 7ac6825c..97cabb60 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -254,7 +254,7 @@ private: #ifdef NAV_MSGS_FOXY void goalResponseCallback(std::shared_future future); #else - void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle); + void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle); #endif void resultCallback(const GoalHandleNav2::WrappedResult & result); #endif @@ -381,6 +381,7 @@ private: #endif #ifdef WITH_NAV2_MSGS rclcpp_action::Client::SharedPtr nav2Client_; + rclcpp_action::GoalUUID lastGoalSent_; #endif std::thread* transformThread_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index e3cd3548..a9da2ce5 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -4436,7 +4436,7 @@ void CoreWrapper::goalResponseCallback( { auto goal_handle = future.get(); #else - const GoalHandleNav2::SharedPtr & goal_handle) + const GoalHandleNav2::SharedPtr & goal_handle) { #endif if (!goal_handle) { @@ -4448,6 +4448,7 @@ void CoreWrapper::goalResponseCallback( latestNodeWasReached_ = false; } else { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + lastGoalSent_ = goal_handle->get_goal_id(); } } @@ -4473,6 +4474,11 @@ void CoreWrapper::resultCallback( RCLCPP_INFO(this->get_logger(), "Planning: nav2 success!"); } } + else if(result.code==rclcpp_action::ResultCode::ABORTED && result.goal_id != lastGoalSent_) + { + // Just ignored, it is from an old goal + ignore = true; + } else { RCLCPP_ERROR(this->get_logger(), "Planning: nav2 failed for some reason: %s. Aborting the plan...", From c21b0ee84e301e119349430afc9af3f87510797c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 11 Sep 2024 23:45:55 -0700 Subject: [PATCH 049/126] map_assembler node is now available on ROS2 --- rtabmap_util/CMakeLists.txt | 12 +- .../include/rtabmap_util/map_assembler.hpp | 104 +++++ rtabmap_util/src/MapAssemblerNode.cpp | 361 +----------------- rtabmap_util/src/nodelets/map_assembler.cpp | 355 +++++++++++++++++ rtabmap_viz/src/GuiWrapper.cpp | 2 +- 5 files changed, 487 insertions(+), 347 deletions(-) create mode 100644 rtabmap_util/include/rtabmap_util/map_assembler.hpp create mode 100644 rtabmap_util/src/nodelets/map_assembler.cpp diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index becf84dd..7ae96252 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -73,6 +73,7 @@ SET(rtabmap_util_plugins_lib_src src/nodelets/lidar_deskewing.cpp src/nodelets/rgbd_relay.cpp src/nodelets/rgbd_split.cpp + src/nodelets/map_assembler.cpp ) @@ -134,6 +135,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudT rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ObstaclesDetection") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAggregator") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAssembler") +rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::MapAssembler") add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp) ament_target_dependencies(rtabmap_rgbd_relay ${Libraries}) @@ -150,10 +152,10 @@ set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") #target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins) #set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer") -#add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) -#ament_target_dependencies(rtabmap_map_assembler ${Libraries}) -#target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins rtabmap_util_plugins) -#set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") +add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) +ament_target_dependencies(rtabmap_map_assembler ${Libraries}) +target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins) +set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp) ament_target_dependencies(rtabmap_imu_to_tf ${Libraries}) @@ -241,7 +243,6 @@ install(TARGETS RUNTIME DESTINATION bin ) install(TARGETS -# rtabmap_map_assembler # rtabmap_map_optimizer # rtabmap_data_player # rtabmap_odom_msg_to_tf @@ -256,6 +257,7 @@ install(TARGETS rtabmap_obstacles_detection rtabmap_point_cloud_aggregator rtabmap_point_cloud_assembler + rtabmap_map_assembler DESTINATION lib/${PROJECT_NAME} ) diff --git a/rtabmap_util/include/rtabmap_util/map_assembler.hpp b/rtabmap_util/include/rtabmap_util/map_assembler.hpp new file mode 100644 index 00000000..8c87adeb --- /dev/null +++ b/rtabmap_util/include/rtabmap_util/map_assembler.hpp @@ -0,0 +1,104 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include "rclcpp/rclcpp.hpp" + +#include +#include "rtabmap_msgs/msg/map_data.hpp" +#include "rtabmap_util/MapsManager.h" + +#include + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +#include +#endif +#endif + +namespace rtabmap_util +{ + +class MapAssembler: public rclcpp::Node +{ + +public: + RTABMAP_UTIL_PUBLIC + explicit MapAssembler(const rclcpp::NodeOptions & options); + virtual ~MapAssembler(); + +private: + void mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg); + + void processMapData(const rtabmap_msgs::msg::MapData & msg); + + void reset(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr); + + void timerCallback(); + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + void octomapBinaryCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res); + + void octomapFullCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res); +#endif +#endif + +private: + MapsManager mapsManager_; + std::map nodes_; + std::map optimizedPoses_; + std::string mapFrameId_; + std::string rtabmapNodeName_; + + rclcpp::Subscription::SharedPtr mapDataSub_; + + rclcpp::Service::SharedPtr resetService_; + + rclcpp::CallbackGroup::SharedPtr serviceCbGroup_; + rclcpp::CallbackGroup::SharedPtr timerCbGroup_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Client::SharedPtr client_; + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + rclcpp::Service::SharedPtr octomapBinarySrv_; + rclcpp::Service::SharedPtr octomapFullSrv_; +#endif +#endif + bool localGridsRegenerated_; +}; + +} \ No newline at end of file diff --git a/rtabmap_util/src/MapAssemblerNode.cpp b/rtabmap_util/src/MapAssemblerNode.cpp index 3b92c221..db37543a 100644 --- a/rtabmap_util/src/MapAssemblerNode.cpp +++ b/rtabmap_util/src/MapAssemblerNode.cpp @@ -25,349 +25,17 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include "rtabmap_msgs/MapData.h" -#include "rtabmap_conversions/MsgConversion.h" -#include "rtabmap_util/MapsManager.h" -#include "rtabmap_msgs/GetMap.h" -#include -#include -#include -#include -#include -#include -#include +#include "rtabmap_util/map_assembler.hpp" + #include -#include -#include -#include -#include -#include -#include -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP -#include -#include -#include -#endif -#endif - -using namespace rtabmap; - -class MapAssembler +int main(int argc, char **argv) { - -public: - MapAssembler(int & argc, char** argv) : - localGridsRegenerated_(false) - { - ros::NodeHandle pnh("~"); - ros::NodeHandle nh; - - std::string configPath; - pnh.param("config_path", configPath, configPath); - pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_); - - //parameters - rtabmap::ParametersMap parameters; - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM")); - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp()))); - if(!configPath.empty()) - { - if(UFile::exists(configPath.c_str())) - { - ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str()); - rtabmap::ParametersMap allParameters; - Parameters::readINI(configPath.c_str(), allParameters); - // only update odometry parameters - for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - ParametersMap::iterator jter = allParameters.find(iter->first); - if(jter!=allParameters.end()) - { - iter->second = jter->second; - } - } - } - else - { - ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str()); - } - } - for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - std::string vStr; - bool vBool; - int vInt; - double vDouble; - if(pnh.getParam(iter->first, vStr)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str()); - iter->second = vStr; - } - else if(pnh.getParam(iter->first, vBool)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); - } - else if(pnh.getParam(iter->first, vDouble)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); - } - else if(pnh.getParam(iter->first, vInt)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); - } - } - - rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv); - for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter) - { - rtabmap::ParametersMap::iterator jter = parameters.find(iter->first); - if(jter!=parameters.end()) - { - ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str()); - jter->second = iter->second; - } - } - - // Backward compatibility - for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); - iter!=Parameters::getRemovedParameters().end(); - ++iter) - { - std::string vStr; - if(pnh.getParam(iter->first, vStr)) - { - if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) - { - // can be migrated - parameters.at(iter->second.second)= vStr; - ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", - ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); - } - else - { - if(iter->second.second.empty()) - { - ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!", - ros::this_node::getName().c_str(), iter->first.c_str()); - } - else - { - ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", - ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str()); - } - } - } - } - - // set private parameters - for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - pnh.setParam(iter->first, iter->second); - } - - ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false"); - mapsManager_.init(nh, pnh, ros::this_node::getName(), false); - mapsManager_.backwardCompatibilityParameters(pnh, parameters); - mapsManager_.setParameters(parameters); - - std::list splitName = uSplit(nh.resolveName("mapData"), '/'); - std::string rtabmapNs; - for(std::list::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter) - { - if(!rtabmapNs.empty()) - { - rtabmapNs += "/"; - } - rtabmapNs += *iter; - } - ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str()); - if(rtabmapNs.empty()) - { - rtabmapNs = "get_map_data"; - } - else - { - rtabmapNs += "/get_map_data"; - } - - rtabmap_msgs::GetMap getMapSrv; - getMapSrv.request.global = false; - getMapSrv.request.optimized = true; - getMapSrv.request.graphOnly = false; - if(ros::service::waitForService(rtabmapNs, 5000)) - { - if(!ros::service::call(rtabmapNs, getMapSrv)) - { - ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str()); - } - else - { - ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str()); - processMapData(getMapSrv.response.data); - ROS_INFO("Called \"%s\" service, initializing cache... done! The map" - " will be assembled on next subscriber connection.", rtabmapNs.c_str()); - } - } - else - { - ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, " - "may not be a problem if rtabmap is started afterwards. If rtabmap " - "is started after in localization mode, call /rtabmap/publish_maps " - "service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str()); - } - - - mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); - - // private services - resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this); - octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this); -#endif -#endif - } - - ~MapAssembler() - { - } - - void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg) - { - processMapData(*msg); - } - void processMapData(const rtabmap_msgs::MapData & msg) - { - UTimer timer; - - std::map poses; - std::multimap constraints; - Transform mapOdom; - rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom); - for(unsigned int i=0; ifirst) != nodes_.end()) - { - Signature tmpS = nodes_.at(poses.rbegin()->first); - SensorData tmpData = tmpS.sensorData(); - tmpData.setId(0); - uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData))); - poses.insert(std::make_pair(0, poses.rbegin()->second)); - } - - // Update maps - if(!nodes_.empty()) - { - poses = mapsManager_.updateMapCaches( - poses, - 0, - false, - false, - nodes_); - } - double updateTime = timer.ticks(); - - mapFrameId_ = msg.header.frame_id; - optimizedPoses_ = poses; - - mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id); - - ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false"); - } - - bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("map_assembler: reset!"); - mapsManager_.clear(); - return true; - } - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - bool octomapBinaryCallback( - octomap_msgs::GetOctomap::Request &req, - octomap_msgs::GetOctomap::Response &res) - { - ROS_INFO("Sending binary map data on service request"); - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); - - const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); - bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); - return success; - } - - bool octomapFullCallback( - octomap_msgs::GetOctomap::Request &req, - octomap_msgs::GetOctomap::Response &res) - { - ROS_INFO("Sending full map data on service request"); - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); - - const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); - bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); - return success; - } -#endif -#endif - -private: - rtabmap_util::MapsManager mapsManager_; - std::map nodes_; - std::map optimizedPoses_; - std::string mapFrameId_; - - ros::Subscriber mapDataTopic_; - - ros::ServiceServer resetService_; -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - ros::ServiceServer octomapBinarySrv_; - ros::ServiceServer octomapFullSrv_; -#endif -#endif - bool localGridsRegenerated_; -}; - - -int main(int argc, char** argv) -{ - ULogger::setLevel(ULogger::kError); ULogger::setType(ULogger::kTypeConsole); - - ros::init(argc, argv, "map_assembler"); + ULogger::setLevel(ULogger::kError); // process "--params" argument + std::vector arguments; for(int i=1;i(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp new file mode 100644 index 00000000..9874ab14 --- /dev/null +++ b/rtabmap_util/src/nodelets/map_assembler.cpp @@ -0,0 +1,355 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include + +#include +#include +#include + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +#include +#include +#endif +#endif + +using namespace std::chrono_literals; + +namespace rtabmap_util +{ +MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : + Node("map_assembler", options), + rtabmapNodeName_("rtabmap"), + localGridsRegenerated_(false) +{ + std::string configPath; + configPath = this->declare_parameter("config_path", configPath); + localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_); + rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); + + //parameters + rtabmap::ParametersMap parameters; + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM")); + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp()))); + if(!configPath.empty()) + { + if(UFile::exists(configPath.c_str())) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Loading parameters from %s", configPath.c_str()); + rtabmap::ParametersMap allParameters; + rtabmap::Parameters::readINI(configPath.c_str(), allParameters); + // only update odometry parameters + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rtabmap::ParametersMap::iterator jter = allParameters.find(iter->first); + if(jter!=allParameters.end()) + { + iter->second = jter->second; + } + } + } + else + { + RCLCPP_ERROR(this->get_logger(), "Config file \"%s\" not found!", configPath.c_str()); + } + } + + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rclcpp::Parameter parameter; + std::string vStr = this->declare_parameter(iter->first, iter->second); + if(vStr.compare(iter->second)!=0) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + } + + std::vector tmpList = this->get_node_options().arguments(); + std::vector argList; + for(unsigned int i=0; i v = uSplit(tmpList[i]); + for(std::list::iterator iter=v.begin(); iter!=v.end(); ++iter) + { + argList.push_back(*iter); + } + } + + char ** argv = new char*[argList.size()]; + for(unsigned int i=0; ifirst); + if(jter!=parameters.end()) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Update parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + jter->second = iter->second; + } + else + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Ignored parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + } + } + + // Backward compatibility + for(std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin(); + iter!=rtabmap::Parameters::getRemovedParameters().end(); + ++iter) + { + rclcpp::Parameter parameter; + if(get_parameter(iter->first, parameter)) + { + std::string vStr = parameter.as_string(); + if(!iter->second.second.empty() && parameters.find(iter->second.second)!=parameters.end()) + { + RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".", + iter->first.c_str(), iter->second.second.c_str(), parameters.find(iter->second.second)->second.c_str(), vStr.c_str()); + } + else if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) + { + // can be migrated + parameters.at(iter->second.second)= vStr; + RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); + } + else + { + if(iter->second.second.empty()) + { + RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } + } + } + } + + RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false"); + mapsManager_.init(*this, this->get_name(), true); + mapsManager_.backwardCompatibilityParameters(*this, parameters); + mapsManager_.setParameters(parameters); + + const std::string servicePrefix = get_name() + std::string("/"); + resetService_ = this->create_service(servicePrefix + "reset", std::bind(&MapAssembler::reset, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&MapAssembler::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&MapAssembler::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); +#endif +#endif + + std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; + + // 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(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer + timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_); +} + +MapAssembler::~MapAssembler() {} + +void MapAssembler::timerCallback() +{ + // Just do this callback one time + timer_->cancel(); + if(mapDataSub_.get()) + { + // double call? ignore + return; + } + + std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; + RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str()); + + if(client_->wait_for_service(5s)) + { + auto request = std::make_shared(); + request->global_map = false; + request->optimized = true; + request->graph_only = false; + + auto future = client_->async_send_request(request); + std::future_status status = future.wait_for(10s); + + if (status == std::future_status::ready) { + RCLCPP_INFO(this->get_logger(), "Initializing cache..."); + processMapData(future.get()->data); + RCLCPP_INFO(this->get_logger(), "Initializing cache... done! The map" + " will be assembled on next subscriber connection."); + } + else + { + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not responding after waiting for 10 seconds.", + getMapSrv.c_str()); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, " + "may not be a problem if rtabmap is started afterwards. If rtabmap " + "is started after in localization mode, call %s/publish_maps " + "service with graph_only=false to make sure map_assembler has all the data.", + getMapSrv.c_str(), + rtabmapNodeName_.c_str()); + } + + rclcpp::SubscriptionOptions options; + options.callback_group = timerCbGroup_; + mapDataSub_ = create_subscription("mapData", rclcpp::QoS(1), + std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options); +} + +void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg) +{ + processMapData(*msg); +} +void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg) +{ + UTimer timer; + + std::map poses; + std::multimap constraints; + rtabmap::Transform mapOdom; + rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom); + for(unsigned int i=0; ifirst) != nodes_.end()) + { + rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first); + rtabmap::SensorData tmpData = tmpS.sensorData(); + tmpData.setId(0); + uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData))); + poses.insert(std::make_pair(0, poses.rbegin()->second)); + } + + // Update maps + if(!nodes_.empty()) + { + poses = mapsManager_.updateMapCaches( + poses, + 0, + false, + false, + nodes_); + } + double updateTime = timer.ticks(); + + mapFrameId_ = msg.header.frame_id; + optimizedPoses_ = poses; + + mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id); + + RCLCPP_INFO(this->get_logger(), "map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false"); +} + +void MapAssembler::reset(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + RCLCPP_INFO(this->get_logger(), "map_assembler: reset!"); + mapsManager_.clear(); +} + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +void MapAssembler::octomapBinaryCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Sending binary map data on service request"); + res->map.header.frame_id = mapFrameId_; + res->map.header.stamp = now(); + + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + if(octomap->octree()->size()) + octomap_msgs::binaryMapToMsg(*octomap->octree(), res->map); +} + +void MapAssembler::octomapFullCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Sending full map data on service request"); + res->map.header.frame_id = mapFrameId_; + res->map.header.stamp = now(); + + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + if(octomap->octree()->size()) + octomap_msgs::fullMapToMsg(*octomap->octree(), res->map); +} +#endif +#endif + +} + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::MapAssembler) diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 0f8cb410..fdb3cd87 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -129,7 +129,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : initCachePath = UDirectory::currentDir(true) + initCachePath; } RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str()); - if(!callMapDataService("get_map_data", false, true, true)) + if(!callMapDataService(rtabmapNodeName_+"/get_map_data", false, true, true)) { RCLCPP_ERROR(this->get_logger(), "The cache will still be loaded " From e0542c7e4b315d3e309f95ea047ac4a2583efd31 Mon Sep 17 00:00:00 2001 From: ChristopherBilberg Date: Sun, 22 Sep 2024 00:51:13 +0200 Subject: [PATCH 050/126] Make sure to use syncQueueSize for sync policies in (#1209) PointCloudAggregator Queue size parameter was split into sync_queue_size and topic_queue_size parameters in ae44e1a2157b84e67ade530f2b8e713eac9650e8, however syncQueueSize was never used in PointCloudAggregator. Co-authored-by: Christopher Prinds Bilberg --- rtabmap_util/src/nodelets/point_cloud_aggregator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 80493365..19ad1886 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -135,14 +135,14 @@ private: cloudSub_4_.subscribe(nh, "cloud4", queueSize); if(approx) { - approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); if(approxSyncMaxInterval > 0.0) approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); exactSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", @@ -159,14 +159,14 @@ private: cloudSub_3_.subscribe(nh, "cloud3", queueSize); if(approx) { - approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); if(approxSyncMaxInterval > 0.0) approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); exactSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", @@ -181,14 +181,14 @@ private: { if(approx) { - approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); if(approxSyncMaxInterval > 0.0) approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); exactSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", From 7819fd47907b65d268468bf059601bdfefbbedf7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 5 Oct 2024 16:03:43 -0700 Subject: [PATCH 051/126] Fixed #1216 --- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 697da963..3b77e680 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -108,9 +108,9 @@ void RGBDOdometry::onOdomInit() int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos()); subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); - if(rgbdCameras <= 0) + if(rgbdCameras < 0) { - rgbdCameras = 1; + rgbdCameras = 0; } keepColor_ = this->declare_parameter("keep_color", keepColor_); From e76cbed9248e3f71072af5a8212a979e439ac9a2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Oct 2024 20:03:14 -0700 Subject: [PATCH 052/126] Fixed build with RTAB-Map 0.21.8 (and grid_map_core dep) --- rtabmap_util/src/MapsManager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 414906f7..be51792a 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1362,7 +1362,11 @@ void MapsManager::publishMaps( (elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_))) { grid_map_msgs::msg::GridMap::UniquePtr msg; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_MINOR>=8) + msg = grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap()); +#else msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap()); +#endif msg->header.frame_id = mapFrameId; msg->header.stamp = stamp; elevationMapPub_->publish(std::move(msg)); From 1950b95bf540d52b44cd2c815824b8238330f3e3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Oct 2024 20:18:00 -0700 Subject: [PATCH 053/126] fixed typo --- rtabmap_util/src/MapsManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index be51792a..24a41da4 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1362,7 +1362,7 @@ void MapsManager::publishMaps( (elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_))) { grid_map_msgs::msg::GridMap::UniquePtr msg; -#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_MINOR>=8) +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8) msg = grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap()); #else msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap()); From c3f7cfeaca8963c0dc11394e914c378839f583e4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Oct 2024 20:18:42 -0700 Subject: [PATCH 054/126] fixed build with RTAB-Map 0.21.8 --- rtabmap_util/src/MapsManager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 614b5f80..1ee902f2 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1485,7 +1485,11 @@ void MapsManager::publishMaps( (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_))) { grid_map_msgs::GridMap msg; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8) + grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap(), msg); +#else grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap(), msg); +#endif msg.info.header.frame_id = mapFrameId; msg.info.header.stamp = stamp; elevationMapPub_.publish(msg); From ad97f56d0b553a8da107628ca0826be51f56e79f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 15 Oct 2024 21:00:35 -0700 Subject: [PATCH 055/126] Multithreaded rtabmap node (#1214) * Multi-threaded rtabmap node * Moved data sync logic inside CoreWrapper to process odometry topics outside processing thread * updated commend * re-enabled jazzy * disabled jazzy for this mr * Added async callbackgroup for imu and landmark topics * Making all shared variables thread-safe --- rtabmap_conversions/src/MsgConversion.cpp | 25 +- .../include/rtabmap_slam/CoreWrapper.h | 42 +- rtabmap_slam/src/CoreNode.cpp | 5 +- rtabmap_slam/src/CoreWrapper.cpp | 722 +++++++++++------- .../rtabmap_sync/CommonDataSubscriber.h | 45 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 18 + .../src/impl/CommonDataSubscriberDepth.cpp | 71 +- .../src/impl/CommonDataSubscriberOdom.cpp | 9 +- .../src/impl/CommonDataSubscriberRGB.cpp | 69 +- .../src/impl/CommonDataSubscriberRGBD.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD2.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD3.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD4.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD5.cpp | 21 +- .../src/impl/CommonDataSubscriberRGBD6.cpp | 21 +- .../src/impl/CommonDataSubscriberRGBDX.cpp | 43 +- .../src/impl/CommonDataSubscriberScan.cpp | 35 +- .../impl/CommonDataSubscriberSensorData.cpp | 9 +- .../src/impl/CommonDataSubscriberStereo.cpp | 15 +- 19 files changed, 785 insertions(+), 537 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 0d820fb4..4bfe58d3 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1691,18 +1691,6 @@ 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; - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size()); - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size()); - for(size_t i=0; i models; - for(size_t j=0; j models; + for(size_t j=0; j > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); + // Callback called from sync thread void commonMultiCameraCallbackImpl( const std::string & odomFrameId, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -148,6 +150,7 @@ private: const std::vector > & localKeyPoints, const std::vector > & localPoints3d, const std::vector & localDescriptors); + // Callback called from sync thread virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -155,11 +158,13 @@ private: const sensor_msgs::msg::PointCloud2 & scan3dMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()); + // Callback called from sync thread virtual void commonOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg); + // Callback called from sync thread virtual void commonSensorDataCallback( const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg, const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, @@ -195,6 +200,8 @@ private: void goalNodeCallback(const rtabmap_msgs::msg::Goal::SharedPtr msg); void updateGoal(const rclcpp::Time & stamp); + void processAsync(); + void process( const rclcpp::Time & stamp, rtabmap::SensorData & data, @@ -266,11 +273,14 @@ private: private: rtabmap::Rtabmap rtabmap_; bool paused_; + + UMutex lastPoseMutex_; rtabmap::Transform lastPose_; rclcpp::Time lastPoseStamp_; std::vector lastPoseVelocity_; + cv::Mat lastPoseCovariance_; bool lastPoseIntermediate_; - cv::Mat covariance_; + rtabmap::Transform currentMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_; bool latestNodeWasReached_; @@ -341,7 +351,7 @@ private: std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; - rclcpp::SyncParametersClient::SharedPtr parametersClient_; + rclcpp::AsyncParametersClient::SharedPtr parametersClient_; rclcpp::Subscription::SharedPtr parameterEventSub_; rclcpp::Service::SharedPtr updateSrv_; @@ -390,6 +400,7 @@ private: // for loop closure detection only image_transport::Subscriber defaultSub_; + rclcpp::CallbackGroup::SharedPtr userDataAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr userDataAsyncSub_; cv::Mat userData_; UMutex userDataMutex_; @@ -398,6 +409,8 @@ private: geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; rtabmap::GPS gps_; + + rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS @@ -407,10 +420,14 @@ private: rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; #endif std::map > landmarks_; // id, - rclcpp::Subscription::SharedPtr imuSub_; + UMutex landmarksMutex_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; std::map imus_; std::string imuFrameId_; + UMutex imuMutex_; + rclcpp::Subscription::SharedPtr republishNodeDataSub_; rclcpp::Subscription::SharedPtr interOdomSub_; @@ -444,6 +461,23 @@ private: double localizationError_; }; LocalizationStatusTask localizationDiagnostic_; + + rclcpp::CallbackGroup::SharedPtr processingCallbackGroup_; + struct SyncData { + bool valid; + rclcpp::Time stamp; + rtabmap::SensorData data; + rtabmap::Transform odom; + std::vector odomVelocity; + std::string odomFrameId; + cv::Mat odomCovariance; + rtabmap::OdometryInfo odomInfo; + double timeMsgConversion; + }; + rclcpp::TimerBase::SharedPtr syncTimer_; + SyncData syncData_; + UMutex syncDataMutex_; + bool triggerNewMapBeforeNextUpdate_; }; } diff --git a/rtabmap_slam/src/CoreNode.cpp b/rtabmap_slam/src/CoreNode.cpp index ec5d226a..d16aa4f1 100644 --- a/rtabmap_slam/src/CoreNode.cpp +++ b/rtabmap_slam/src/CoreNode.cpp @@ -84,8 +84,11 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); UINFO("rtabmap %s started...", RTABMAP_VERSION); - rclcpp::spin(std::make_shared(options)); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index a9da2ce5..508804a9 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -139,7 +139,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()), twoDMapping_(Parameters::defaultRegForce3DoF()), previousStamp_(0), - ulogToRosout_(this) + ulogToRosout_(this), + triggerNewMapBeforeNextUpdate_(false) { char * rosHomePath = getenv("ROS_HOME"); std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros"; @@ -148,6 +149,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : mapsManager_.init(*this, this->get_name(), true); + syncData_.valid = false; + tfBuffer_ = std::make_shared(this->get_clock()); //auto timer_interface = std::make_shared( // this->get_node_base_interface(), @@ -271,6 +274,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(get_logger(), "rtabmap: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false"); } + // Create the processing timer + processingCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + syncTimer_ = this->create_wall_timer(0s, std::bind(&CoreWrapper::processAsync, this), processingCallbackGroup_); + syncTimer_->cancel(); + + rclcpp::SubscriptionOptions subOptions; + subOptions.callback_group = processingCallbackGroup_; + infoPub_ = this->create_publisher("info", 1); mapDataPub_ = this->create_publisher("mapData", 1); mapGraphPub_ = this->create_publisher("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); @@ -282,11 +293,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : localGridEmpty_ = this->create_publisher("local_grid_empty", 1); localGridGround_ = this->create_publisher("local_grid_ground", 1); localizationPosePub_ = this->create_publisher("localization_pose", 1); - initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1)); + initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1), subOptions); // planning topics - goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1)); - goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1)); + goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1), subOptions); + goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1), subOptions); nextMetricGoalPub_ = this->create_publisher("goal_out", 1); goalReachedPub_ = this->create_publisher("goal_reached", 1); globalPathPub_ = this->create_publisher("global_path", 1); @@ -560,7 +571,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : else { RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom messages"); - interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1)); + interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1), subOptions); } } @@ -649,45 +660,45 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : // setup services const std::string servicePrefix = get_name() + std::string("/"); - updateSrv_ = this->create_service(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - triggerNewMapSrv_ = this->create_service(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - backupDatabase_ = this->create_service(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - detectMoreLoopClosuresSrv_ = this->create_service(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - globalBundleAdjustmentSrv_ = this->create_service(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cleanupLocalGridsSrv_ = this->create_service(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeLocalizationSrv_ = this->create_service(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeMappingSrv_ = this->create_service(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodeDataSrv_ = this->create_service(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapDataSrv_ = this->create_service(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapData2Srv_ = this->create_service(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapSrv_ = this->create_service(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getProbMapSrv_ = this->create_service(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - publishMapDataSrv_ = this->create_service(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanNodesSrv_ = this->create_service(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setGoalSrv_ = this->create_service(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodesInRadiusSrv_ = this->create_service(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + updateSrv_ = this->create_service(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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_); #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapBinarySrv_ = this->create_service(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(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); #endif #endif //private services - setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogDebugSrv_ = this->create_service(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(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(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(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); @@ -700,9 +711,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rclcpp::Rate r(1.0 / tfDelay); while(tfThreadRunning_) { + mapToOdomMutex_.lock(); if(!odomFrameId_.empty()) { - mapToOdomMutex_.lock(); rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance); geometry_msgs::msg::TransformStamped msg; msg.child_frame_id = odomFrameId_; @@ -710,8 +721,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : msg.header.stamp = tfExpiration; rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); tfBroadcaster_->sendTransform(msg); - mapToOdomMutex_.unlock(); } + mapToOdomMutex_.unlock(); r.sleep(); } }); @@ -748,7 +759,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } auto node = rclcpp::Node::make_shared("rtabmap"); image_transport::TransportHints hints(this); - defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile()); + defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); @@ -834,25 +845,36 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } this->set_parameters(rosParameters); + // Setup callback groups for any subscriptions that should not be affected by main processing thread. + userDataAsyncCallbackGroup_ = 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 landmarkSubOptions; + rclcpp::SubscriptionOptions imuSubOptions; + userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_; + landmarkSubOptions.callback_group = imuCallbackGroup_; + imuSubOptions.callback_group = imuCallbackGroup_; + int qosGPS = 0; int qosIMU = 0; qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); - userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1)); - globalPoseAsyncSub_ = this->create_subscription("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1)); - gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionSub_ = this->create_subscription("landmark_detection", 5, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 5, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1)); + userDataAsyncSub_ = this->create_subscription("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("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions); + gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), subOptions); + landmarkDetectionSub_ = this->create_subscription("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS - tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1)); + tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS - fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1)); + fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif - imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1)); - republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1)); + imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1), imuSubOptions); + republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 1, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1), subOptions); - parametersClient_ = std::make_shared(this); + parametersClient_ = std::make_shared(this, std::string(), rmw_qos_profile_parameters, processingCallbackGroup_); auto on_parameter_event_callback = [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void { @@ -908,7 +930,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : }; // Setup callback for changes to parameters. - parameterEventSub_ = parametersClient_->on_parameter_event(on_parameter_event_callback); + rclcpp::SubscriptionOptionsWithAllocator> paramOptions; + paramOptions.callback_group = processingCallbackGroup_; + parameterEventSub_ = parametersClient_->on_parameter_event( + on_parameter_event_callback, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)), + paramOptions); } CoreWrapper::~CoreWrapper() @@ -1071,11 +1098,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti } } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg.pose.covariance[0] >= BAD_COVARIANCE && odomMsg.twist.covariance[0] >= BAD_COVARIANCE))) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0])); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1110,9 +1139,9 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti covariance.at(0,0)>0.0) { // Use largest covariance error (to be independent of the odometry frame rate) - if(covariance_.empty() || covariance.at(0,0) > covariance_.at(0,0)) + if(lastPoseCovariance_.empty() || covariance.at(0,0) > lastPoseCovariance_.at(0,0)) { - covariance_ = covariance; + lastPoseCovariance_ = covariance; } } } @@ -1142,32 +1171,30 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } return false; } -bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) +bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp) { if(!paused_) { // Odom TF ready? - Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, *tfBuffer_, waitForTransform_); + Transform odom = rtabmap_conversions::getTransform(odomFrameId, frameId_, stamp, *tfBuffer_, waitForTransform_); if(odom.isNull()) { return false; } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && odom.isIdentity()) { UWARN("Odometry is reset (identity pose detected). Increment map id!"); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1199,10 +1226,6 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } @@ -1224,7 +1247,7 @@ void CoreWrapper::commonMultiCameraCallback( const std::vector > & localPoints3d, const std::vector & localDescriptors) { - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1247,38 +1270,53 @@ void CoreWrapper::commonMultiCameraCallback( return; } } - else if(!scan2dMsg.ranges.empty()) + else { - if(!odomTFUpdate(scan2dMsg.header.stamp)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) + { + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } + } + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(odomFrameId, cameraInfoMsgs[0].header.stamp)) { return; } } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) - { - return; - } - } - else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp)) - { - return; - } - commonMultiCameraCallbackImpl(odomFrameId, - userDataMsg, - imageMsgs, - depthMsgs, - cameraInfoMsgs, - depthCameraInfoMsgs, - scan2dMsg, - scan3dMsg, - odomInfoMsg, - globalDescriptorMsgs, - localKeyPoints, - localPoints3d, - localDescriptors); + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) + { + UScopeMutex lock(lastPoseMutex_); + commonMultiCameraCallbackImpl(odomFrameId, + userDataMsg, + imageMsgs, + depthMsgs, + cameraInfoMsgs, + depthCameraInfoMsgs, + scan2dMsg, + scan3dMsg, + odomInfoMsg, + globalDescriptorMsgs, + localKeyPoints, + localPoints3d, + localDescriptors); + + if(syncData_.valid) { + syncTimer_->reset(); + } + syncDataMutex_.unlock(); + } } void CoreWrapper::commonMultiCameraCallbackImpl( @@ -1528,10 +1566,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl( userData_ = cv::Mat(); } - SensorData data; if(!stereoCameraModels.empty()) { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1542,7 +1579,7 @@ void CoreWrapper::commonMultiCameraCallbackImpl( } else { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1560,25 +1597,31 @@ void CoreWrapper::commonMultiCameraCallbackImpl( if(!globalDescriptorMsgs.empty()) { - data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); + syncData_.data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); } if(!keypoints.empty()) { UASSERT(points.empty() || points.size() == keypoints.size()); UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size()); - data.setFeatures(keypoints, points, descriptors); + syncData_.data.setFeatures(keypoints, points, descriptors); } - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - covariance_ = cv::Mat(); + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); } void CoreWrapper::commonLaserScanCallback( @@ -1590,7 +1633,7 @@ void CoreWrapper::commonLaserScanCallback( const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor) { UTimer timerConversion; - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1613,110 +1656,128 @@ void CoreWrapper::commonLaserScanCallback( return; } } - else if(!scan2dMsg.ranges.empty()) - { - if(!odomTFUpdate(scan2dMsg.header.stamp)) - { - return; - } - } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) - { - return; - } - } else { - return; - } - - LaserScan scan; - if(!scan2dMsg.ranges.empty()) - { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - // backward compatibility, project 2D scan in /base_link frame - rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); - return; + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - scanCloudMaxPoints_, - 0, - scanCloudIs2d_)) + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); return; } } - cv::Mat userData; - if(userDataMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + UScopeMutex lock(lastPoseMutex_); + LaserScan scan; + if(!scan2dMsg.ranges.empty()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + if(!rtabmap_conversions::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + // backward compatibility, project 2D scan in /base_link frame + rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); + return; + } + } + else if(!scan3dMsg.data.empty()) + { + if(!rtabmap_conversions::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + scanCloudMaxPoints_, + 0, + scanCloudIs2d_)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); + return; + } + } + + cv::Mat userData; + if(userDataMsg.get()) + { + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } + + syncData_.data = SensorData( + scan, + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + if(!globalDescriptor.data.empty()) + { + syncData_.data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - - SensorData data( - scan, - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - - if(!globalDescriptor.data.empty()) - { - data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); - } - - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); } void CoreWrapper::commonOdomCallback( @@ -1726,56 +1787,66 @@ void CoreWrapper::commonOdomCallback( { UTimer timerConversion; UASSERT(odomMsg.get()); - std::string odomFrameId = odomFrameId_; - - odomFrameId = odomMsg->header.frame_id; + std::string odomFrameId = odomMsg->header.frame_id; if(!odomUpdate(*odomMsg, odomMsg->header.stamp)) { return; } - cv::Mat userData; - if(userDataMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + UScopeMutex lock(lastPoseMutex_); + cv::Mat userData; + if(userDataMsg.get()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } + + syncData_.data = SensorData( + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - - SensorData data( - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); } void CoreWrapper::commonSensorDataCallback( @@ -1785,7 +1856,7 @@ void CoreWrapper::commonSensorDataCallback( { UTimer timerConversion; UASSERT(sensorDataMsg.get()); - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1794,30 +1865,73 @@ void CoreWrapper::commonSensorDataCallback( return; } } - else if(!odomTFUpdate(sensorDataMsg->header.stamp)) + else { - return; + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) + { + return; + } } - SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); - data.setId(lastPoseIntermediate_?-1:0); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); + UScopeMutex lock(lastPoseMutex_); + syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); + syncData_.data.setId(lastPoseIntermediate_?-1:0); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); + } +} + +void CoreWrapper::processAsync() +{ + UScopeMutex lock(syncDataMutex_); + + if(triggerNewMapBeforeNextUpdate_) + { + rtabmap_.triggerNewMap(); + triggerNewMapBeforeNextUpdate_ = false; } - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); + if(syncData_.valid) + { + process(syncData_.stamp, + syncData_.data, + syncData_.odom, + syncData_.odomVelocity, + syncData_.odomFrameId, + syncData_.odomCovariance, + syncData_.odomInfo, + syncData_.timeMsgConversion); + syncData_.valid=false; + } + syncTimer_->cancel(); } void CoreWrapper::process( @@ -1836,7 +1950,7 @@ void CoreWrapper::process( // Add intermediate nodes? for(std::list >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) { - if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < lastPoseStamp_) + if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp) { Transform interOdom; if(!rtabmap_.getLocalOptimizedPoses().empty()) @@ -1925,7 +2039,7 @@ void CoreWrapper::process( } interOdoms_.erase(iter++); } - else if(iter->first.header.stamp == lastPoseStamp_) + else if(iter->first.header.stamp == stamp) { interOdoms_.erase(iter++); break; @@ -1940,7 +2054,7 @@ void CoreWrapper::process( Transform groundTruthPose; if(!groundTruthFrameId_.empty()) { - groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, *tfBuffer_, waitForTransform_); + groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp, *tfBuffer_, waitForTransform_); } data.setGroundTruth(groundTruthPose); @@ -1951,7 +2065,7 @@ void CoreWrapper::process( Transform sensorToBase = rtabmap_conversions::getTransform( globalPose_.header.frame_id, frameId_, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_); if(!sensorToBase.isNull()) @@ -1963,7 +2077,7 @@ void CoreWrapper::process( Transform correction = rtabmap_conversions::getMovingTransform( frameId_, odomFrameId, - lastPoseStamp_, + stamp, rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec), *tfBuffer_, waitForTransform_); @@ -1990,27 +2104,31 @@ void CoreWrapper::process( gps_ = rtabmap::GPS(); //tag detections + landmarksMutex_.lock(); Landmarks landmarks = rtabmap_conversions::landmarksFromROS( landmarks_, frameId_, odomFrameId, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_, landmarkDefaultLinVariance_, landmarkDefaultAngVariance_); landmarks_.clear(); + landmarksMutex_.unlock(); if(!landmarks.empty()) { data.setLandmarks(landmarks); } // IMU + imuMutex_.lock(); if(!imus_.empty()) { Transform t = Transform::getTransform(imus_, data.stamp()); if(!t.isNull()) { + imuMutex_.unlock(); // get local transform rtabmap::Transform localTransform; if(frameId_.compare(imuFrameId_) != 0) @@ -2034,10 +2152,15 @@ void CoreWrapper::process( else { RCLCPP_WARN(this->get_logger(), "We are receiving imu data (buffer=%d), but cannot interpolate " - "imu transform at time %f. IMU won't be added to graph.", - (int)imus_.size(), data.stamp()); + "imu transform at time %f (latest imu received with stamp %f). IMU won't be added to graph.", + (int)imus_.size(), data.stamp(), imus_.rbegin()->first); + imuMutex_.unlock(); } } + else + { + imuMutex_.unlock(); + } double timeRtabmap = 0.0; double timeUpdateMaps = 0.0; @@ -2103,7 +2226,7 @@ void CoreWrapper::process( timeRtabmap = timer.ticks(); mapToOdomMutex_.lock(); mapToOdom_ = rtabmap_.getMapCorrection(); - + Transform mapToOdomSafe = mapToOdom_.clone(); if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0) { RCLCPP_ERROR(get_logger(), "Odometry received doesn't have same frame_id " @@ -2132,7 +2255,7 @@ void CoreWrapper::process( geometry_msgs::msg::PoseWithCovarianceStamped poseMsg; poseMsg.header.frame_id = mapFrameId_; poseMsg.header.stamp = stamp; - rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); + rtabmap_conversions::transformToPoseMsg(mapToOdomSafe*odom, poseMsg.pose.pose); if(!rtabmap_.getStatistics().localizationCovariance().empty()) { const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); @@ -2165,12 +2288,12 @@ void CoreWrapper::process( SensorData tmpData = data; tmpData.setId(0); tmpSignature.insert(std::make_pair(0, Signature(0, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); - filteredPoses.insert(std::make_pair(0, mapToOdom_*odom)); + filteredPoses.insert(std::make_pair(0, mapToOdomSafe*odom)); } if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1) { - std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdom_*odom); + std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdomSafe*odom); //add latest/zero and make sure those on a planned path are not filtered std::set onPath; if(rtabmap_.getPath().size()) @@ -2324,7 +2447,7 @@ void CoreWrapper::process( timeRtabmap, timeUpdateMaps, timePublishMaps, - (now() - lastPoseStamp_).seconds(), + (now() - stamp).seconds(), (int)rtabmap_.getLocalOptimizedPoses().size(), rtabmap_.getWMSize()+rtabmap_.getSTMSize()); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0)); @@ -2431,6 +2554,7 @@ void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::Landma geometry_msgs::msg::PoseWithCovarianceStamped p; p.header = landmarkDetection->header; p.pose = landmarkDetection->pose; + UScopeMutex lock(landmarksMutex_); uInsert(landmarks_, std::make_pair(landmarkDetection->id, std::make_pair(p, landmarkDetection->size))); @@ -2441,6 +2565,7 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; ilandmarks.size(); ++i) { geometry_msgs::msg::PoseWithCovarianceStamped p; @@ -2458,6 +2583,7 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; idetections.size(); ++i) { std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id); @@ -2493,6 +2619,7 @@ void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::Fidu { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; iorientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); imus_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(msg->header.stamp), orientation)); if(imus_.size() > 1000) @@ -2829,10 +2957,15 @@ void CoreWrapper::resetRtabmapCallback( { RCLCPP_INFO(this->get_logger(), "rtabmap: Reset"); rtabmap_.resetMemory(); - covariance_ = cv::Mat(); + + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2842,12 +2975,16 @@ void CoreWrapper::resetRtabmapCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -2923,10 +3060,14 @@ void CoreWrapper::loadDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2936,12 +3077,16 @@ void CoreWrapper::loadDatabaseCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -3050,9 +3195,14 @@ void CoreWrapper::backupDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(this->get_logger(), "Backup: Saving memory... done!"); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); + lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -3063,7 +3213,9 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.unlock(); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); RCLCPP_INFO(this->get_logger(), "Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back"); @@ -3388,11 +3540,15 @@ void CoreWrapper::getMapDataCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3432,11 +3588,15 @@ void CoreWrapper::getMapData2Callback( req->with_words, req->with_global_descriptors); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3555,6 +3715,10 @@ void CoreWrapper::publishMapCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + if(mapDataPub_->get_subscription_count()) { rtabmap_msgs::msg::MapData::UniquePtr msg(new rtabmap_msgs::msg::MapData); @@ -3564,7 +3728,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, *msg); mapDataPub_->publish(std::move(msg)); @@ -3578,7 +3742,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapGraphToROS(poses, constraints, - mapToOdom_, + mapToOdomSafe, *msg); mapGraphPub_->publish(std::move(msg)); diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index f8c58a69..20e5f2b4 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -117,26 +117,27 @@ protected: const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0; - void commonSingleCameraCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr & imageMsg, - const cv_bridge::CvImageConstPtr & depthMsg, - const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, - const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, - const sensor_msgs::msg::LaserScan & scanMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector & globalDescriptorMsgs = std::vector(), - const std::vector & localKeyPoints = std::vector(), - const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); - void tick(const rclcpp::Time & stamp, double targetFrequency = 0); private: + void commonSingleCameraCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const cv_bridge::CvImageConstPtr & imageMsg, + const cv_bridge::CvImageConstPtr & depthMsg, + const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, + const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs = std::vector(), + const std::vector & localKeyPoints = std::vector(), + const std::vector & localPoints3d = std::vector(), + const cv::Mat & localDescriptors = cv::Mat()); + void processSyncData(); void setupDepthCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -145,10 +146,12 @@ private: bool subscribeOdomInfo); void setupStereoCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo); void setupRGBCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -157,6 +160,7 @@ private: bool subscribeOdomInfo); void setupRGBDCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -165,6 +169,7 @@ private: bool subscribeOdomInfo); void setupRGBDXCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -174,6 +179,7 @@ private: #ifdef RTABMAP_SYNC_MULTI_RGBD void setupRGBD2Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -182,6 +188,7 @@ private: bool subscribeOdomInfo); void setupRGBD3Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -190,6 +197,7 @@ private: bool subscribeOdomInfo); void setupRGBD4Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -198,6 +206,7 @@ private: bool subscribeOdomInfo); void setupRGBD5Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -206,6 +215,7 @@ private: bool subscribeOdomInfo); void setupRGBD6Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -215,10 +225,12 @@ private: #endif void setupSensorDataCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo); void setupScanCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeScan2d, bool subscribeScanDesc, bool subscribeOdom, @@ -226,6 +238,7 @@ private: bool subscribeOdomInfo); void setupOdomCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, bool subscribeOdomInfo); @@ -257,6 +270,8 @@ private: int rgbdCameras_; std::string name_; + rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; + //for depth and rgb-only callbacks image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageDepthSub_; diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 746510a5..c714e930 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -363,6 +363,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : { name_ = node.get_name(); + syncCallbackGroup_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // ROS related parameters (private) // ros2: should be declared in the constructor to be used by inherited classes in their constructor subscribedToDepth_ = node.declare_parameter("subscribe_depth", subscribedToDepth_); @@ -539,11 +541,15 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_); RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false"); + rclcpp::SubscriptionOptions callbackOptions; + callbackOptions.callback_group = syncCallbackGroup_; + subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_; if(subscribedToDepth_) { setupDepthCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -555,6 +561,7 @@ void CommonDataSubscriber::setupCallbacks( { setupStereoCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToOdomInfo_); } @@ -562,6 +569,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -583,6 +591,7 @@ void CommonDataSubscriber::setupCallbacks( setupRGBD6Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -594,6 +603,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD5Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -605,6 +615,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD4Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -616,6 +627,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD3Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -627,6 +639,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD2Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -647,6 +660,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBDXCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -658,6 +672,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBDCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -670,6 +685,7 @@ void CommonDataSubscriber::setupCallbacks( { setupScanCallbacks( node, + callbackOptions, subscribedToScan2d_, subscribedToScanDescriptor_, subscribedToOdom_, @@ -680,6 +696,7 @@ void CommonDataSubscriber::setupCallbacks( { setupSensorDataCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToOdomInfo_); } @@ -687,6 +704,7 @@ void CommonDataSubscriber::setupCallbacks( { setupOdomCallbacks( node, + callbackOptions, subscribedToUserData_, subscribedToOdomInfo_); } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 62782fea..30046ed0 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -457,6 +457,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupDepthCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -471,24 +472,24 @@ void CommonDataSubscriber::setupDepthCallbacks( RCLCPP_INFO(node.get_logger(), "Setup depth callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -499,11 +500,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -514,11 +515,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -529,7 +530,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -541,16 +542,16 @@ void CommonDataSubscriber::setupDepthCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -561,11 +562,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -576,11 +577,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -591,7 +592,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -602,17 +603,17 @@ void CommonDataSubscriber::setupDepthCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -623,12 +624,12 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -639,11 +640,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -654,7 +655,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -668,11 +669,11 @@ void CommonDataSubscriber::setupDepthCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -683,11 +684,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -698,11 +699,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -713,7 +714,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp index 573b49ce..6edc2f49 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -65,6 +65,7 @@ void CommonDataSubscriber::odomDataInfoCallback( void CommonDataSubscriber::setupOdomCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, bool subscribeOdomInfo) { @@ -72,16 +73,16 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeUserData || subscribeOdomInfo) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_); } else @@ -94,7 +95,7 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_); } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index e21b80ae..f6a02e26 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -457,6 +457,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupRGBCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -471,23 +472,23 @@ void CommonDataSubscriber::setupRGBCallbacks( RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -498,11 +499,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -513,11 +514,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -528,7 +529,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -540,16 +541,16 @@ void CommonDataSubscriber::setupRGBCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -560,11 +561,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -575,11 +576,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -590,7 +591,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -601,17 +602,17 @@ void CommonDataSubscriber::setupRGBCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -622,12 +623,12 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -638,11 +639,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -653,7 +654,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -667,11 +668,11 @@ void CommonDataSubscriber::setupRGBCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -682,11 +683,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -697,11 +698,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -712,7 +713,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index 7cd6eb23..6e81e827 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -531,6 +531,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -555,17 +556,17 @@ void CommonDataSubscriber::setupRGBDCallbacks( { rgbdSubs_.resize(1); rgbdSubs_[0] = new message_filters::Subscriber; - rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -576,7 +577,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -587,7 +588,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -598,7 +599,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -610,11 +611,11 @@ void CommonDataSubscriber::setupRGBDCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -625,7 +626,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -636,7 +637,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -647,7 +648,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -658,11 +659,11 @@ void CommonDataSubscriber::setupRGBDCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -673,7 +674,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -684,7 +685,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -695,7 +696,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -709,7 +710,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -720,7 +721,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -731,7 +732,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -742,7 +743,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index fe7e1a5b..5f708f43 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -343,6 +343,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD2Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -360,17 +361,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks( for(int i=0; i<2; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -381,7 +382,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -392,7 +393,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -403,7 +404,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -415,11 +416,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -430,7 +431,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -441,7 +442,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -452,7 +453,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -463,11 +464,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -478,7 +479,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -489,7 +490,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -500,7 +501,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -514,7 +515,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -525,7 +526,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -536,7 +537,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -547,7 +548,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index 81116764..e175357f 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -431,6 +431,7 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD3Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -448,17 +449,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks( for(int i=0; i<3; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -469,7 +470,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -480,7 +481,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -491,7 +492,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -503,11 +504,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -518,7 +519,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -529,7 +530,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -540,7 +541,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -551,11 +552,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -566,7 +567,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -577,7 +578,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -588,7 +589,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -602,7 +603,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -613,7 +614,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -624,7 +625,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; @@ -636,7 +637,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index 6c276809..edd5704f 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -400,6 +400,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD4Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -417,17 +418,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks( for(int i=0; i<4; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -438,7 +439,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -449,7 +450,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -460,7 +461,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -472,11 +473,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -487,7 +488,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -498,7 +499,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -509,7 +510,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -520,11 +521,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -535,7 +536,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -546,7 +547,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -557,7 +558,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -571,7 +572,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -582,7 +583,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -593,7 +594,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -604,7 +605,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index 16befb5a..e14dd915 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -256,6 +256,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback( void CommonDataSubscriber::setupRGBD5Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, @@ -269,15 +270,15 @@ void CommonDataSubscriber::setupRGBD5Callbacks( for(int i=0; i<5; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -288,7 +289,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -299,7 +300,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -310,7 +311,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else @@ -323,7 +324,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -334,7 +335,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -345,7 +346,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -356,7 +357,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index a5cd0b8c..320cdfb2 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -274,6 +274,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback( void CommonDataSubscriber::setupRGBD6Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, @@ -287,15 +288,15 @@ void CommonDataSubscriber::setupRGBD6Callbacks( for(int i=0; i<6; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -306,7 +307,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -317,7 +318,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -328,7 +329,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else @@ -341,7 +342,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -352,7 +353,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -363,7 +364,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -374,7 +375,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index 66af728c..02affd03 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -319,6 +319,7 @@ void CommonDataSubscriber::rgbdXOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDXCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -332,16 +333,16 @@ void CommonDataSubscriber::setupRGBDXCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback"); - rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -352,7 +353,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -363,7 +364,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -374,7 +375,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); } else @@ -386,11 +387,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -401,7 +402,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -412,7 +413,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -423,7 +424,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_); } else @@ -434,11 +435,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -449,7 +450,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -460,7 +461,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -471,7 +472,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_); } else @@ -485,7 +486,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -496,7 +497,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -507,7 +508,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -518,7 +519,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp index cf8dd758..8f8cd390 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -245,6 +245,7 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback( void CommonDataSubscriber::setupScanCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool scan2dTopic, bool scanDescTopic, bool subscribeOdom, @@ -266,31 +267,31 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else if(scan2dTopic) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); } else @@ -303,7 +304,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_); } else @@ -316,7 +317,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); } else @@ -329,14 +330,14 @@ void CommonDataSubscriber::setupScanCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_); } else @@ -349,7 +350,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_); } else @@ -362,7 +363,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_); } else @@ -374,14 +375,14 @@ void CommonDataSubscriber::setupScanCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_); } else @@ -394,7 +395,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_); } else @@ -407,7 +408,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_); } else @@ -420,7 +421,7 @@ void CommonDataSubscriber::setupScanCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_); diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index 5c537a51..fff29579 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -65,19 +65,20 @@ void CommonDataSubscriber::sensorDataOdomInfoCallback( void CommonDataSubscriber::setupSensorDataCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup SensorData callback"); - sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile()); + sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_); } else @@ -90,7 +91,7 @@ void CommonDataSubscriber::setupSensorDataCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 2832ca24..13ced404 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -87,25 +87,26 @@ void CommonDataSubscriber::stereoOdomInfoCallback( void CommonDataSubscriber::setupStereoCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); image_transport::TransportHints hints(&node); - imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else @@ -118,7 +119,7 @@ void CommonDataSubscriber::setupStereoCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else From c83cff14c9479f5cffe2553b2dc0bdf7cc067018 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Thu, 17 Oct 2024 00:04:25 -0400 Subject: [PATCH 056/126] update realsense examples for ros2 (#1224) * update realsense examples * Modifiying realsense2 namespace instead. * single quote str --------- Co-authored-by: matlabbe --- rtabmap_examples/launch/realsense_d435i_color.launch.py | 7 +++---- rtabmap_examples/launch/realsense_d435i_infra.launch.py | 5 +++-- rtabmap_examples/launch/realsense_d435i_stereo.launch.py | 5 +++-- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index 298b800f..276673ea 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -2,8 +2,6 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true -# # $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py import os @@ -39,9 +37,10 @@ def generate_launch_description(): PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('realsense2_camera'), 'launch'), '/rs_launch.py']), - launch_arguments={'enable_gyro': 'true', + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '1', + 'unite_imu_method': '2', 'align_depth.enable': 'true', 'enable_sync': 'true', 'rgb_camera.profile': '640x360x30'}.items(), diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index 140e995b..91013e21 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -37,9 +37,10 @@ def generate_launch_description(): PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('realsense2_camera'), 'launch'), '/rs_launch.py']), - launch_arguments={'enable_gyro': 'true', + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '1', + 'unite_imu_method': '2', 'enable_infra1': 'true', 'enable_infra2': 'true', 'enable_sync': 'true'}.items(), diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index ce879554..944f9637 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -37,9 +37,10 @@ def generate_launch_description(): PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('realsense2_camera'), 'launch'), '/rs_launch.py']), - launch_arguments={'enable_gyro': 'true', + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '1', + 'unite_imu_method': '2', 'enable_infra1': 'true', 'enable_infra2': 'true', 'enable_sync': 'true'}.items(), From 817417e11609a372be9ebafe6edc0e7cd6ed5723 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 19 Oct 2024 21:07:51 -0700 Subject: [PATCH 057/126] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ce68bdb0..48c48fd3 100644 --- a/README.md +++ b/README.md @@ -59,9 +59,9 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa # Installation ## ROS2 distribution -**Under construction**: see [ros2 branch](https://github.com/introlab/rtabmap_ros/tree/ros2#rtabmap_ros). +See [ros2 branch](https://github.com/introlab/rtabmap_ros/tree/ros2#rtabmap_ros). -## ROS distribution +## ROS1 distribution RTAB-Map is released as binaries in the ROS distribution. ```bash From ddd5e195cac0e1c597ed3f5c11c15289936ca4cc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 19 Oct 2024 21:10:08 -0700 Subject: [PATCH 058/126] Update README.md --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 48c48fd3..bf16e219 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Build Status - ROS 2 + ROS 2 Humble Build Status @@ -42,6 +42,10 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Iron Build Status + + Jazzy + Build Status + Rolling Build Status From e9aa8ed082a2669f52186d2802645bb1237634f8 Mon Sep 17 00:00:00 2001 From: chcaya <55998117+chcaya@users.noreply.github.com> Date: Thu, 31 Oct 2024 23:35:56 -0400 Subject: [PATCH 059/126] Fixed callbackCloud warning (#1233) * Fixed callbackCloud warning * reverted warning order --------- Co-authored-by: chcaya Co-authored-by: matlabbe --- rtabmap_util/src/nodelets/point_cloud_assembler.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 2264af39..fa1b95d8 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -286,13 +286,14 @@ void PointCloudAssembler::callbackCloudOdomInfo( } else { - RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received."); + RCLCPP_WARN(this->get_logger(), "Resetting point cloud assembler as null odometry has been received."); clouds_.clear(); } } void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { + callbackCalled_ = true; if(cloudPub_->get_subscription_count()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, From debfb0759ed97666012e8159a13b19e500ae8704 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 31 Oct 2024 20:37:38 -0700 Subject: [PATCH 060/126] Applying https://github.com/introlab/rtabmap_ros/pull/1233 to ros1 --- rtabmap_util/src/nodelets/point_cloud_assembler.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index ead6cb0e..a60779a8 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -302,6 +302,7 @@ private: void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { + callbackCalled_ = true; if(cloudPub_.getNumSubscribers()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, From 3f6adad46313a8b903a80d6de94f76a53a35086d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 30 Nov 2024 17:28:16 -0800 Subject: [PATCH 061/126] ROS2 various QOL updates (preparing new binary release) (#1225) * Updating examples and README * updated main readme * odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning. * update readme * Added husky demo * converted demo_robot_mapping.launch to ros2 * Converted stereo_outdoor demo from ros1 to ros2 * Converted multi-session demo to ros2 * Converted demo_find_object launch to ros2 * renamed files * uniformized default qos, increased sync_queue_size default to 10 * Added vlp16 + imu example * Added husky 3D lidar demos * moved demos in subdir * Added vlp16+zed example * rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo. * forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo * pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces * Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them. * Added isaac sim demo * Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty. * Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo. * rtabmap_demos: added README with examples * Added TOC * bump version to 0.21.9 --- .gitignore | 1 + README.md | 79 +--- rtabmap_conversions/package.xml | 2 +- rtabmap_conversions/src/MsgConversion.cpp | 9 +- rtabmap_demos/CMakeLists.txt | 2 +- rtabmap_demos/README.md | 101 +++++ rtabmap_demos/config/demo_robot_mapping.rviz | 391 ++++++++++++++++++ rtabmap_demos/config/find_object.ini | 188 +++++++++ rtabmap_demos/data/books/4.png | Bin 0 -> 23569 bytes rtabmap_demos/data/books/5.png | Bin 0 -> 19764 bytes rtabmap_demos/data/books/6.png | Bin 0 -> 18321 bytes rtabmap_demos/data/books/7.png | Bin 0 -> 13749 bytes rtabmap_demos/data/books/8.png | Bin 0 -> 16528 bytes .../launch/champ/champ_sim_vslam.launch.py | 87 ++++ .../launch/champ/champ_vslam.launch.py | 179 ++++++++ .../launch/find_object_demo.launch.py | 129 ++++++ .../husky/husky_sim_scan2d_demo.launch.py | 104 +++++ .../husky_sim_scan3d_assemble_demo.launch.py | 101 +++++ .../husky/husky_sim_scan3d_demo.launch.py | 104 +++++ .../launch/husky/husky_slam2d.launch.py | 128 ++++++ .../launch/husky/husky_slam3d.launch.py | 138 +++++++ .../husky/husky_slam3d_assemble.launch.py | 134 ++++++ .../isaac/isaac_sim_vslam_demo.launch.py | 280 +++++++++++++ .../launch/isaac/isaac_vslam.launch.py | 139 +++++++ .../multisession_mapping_demo.launch.py | 115 ++++++ .../launch/robot_mapping_demo.launch.py | 112 +++++ .../launch/stereo_outdoor_demo.launch.py | 155 +++++++ .../turtlebot3_rgbd.launch.py | 64 ++- .../turtlebot3_rgbd_scan.launch.py} | 72 ++-- .../turtlebot3_scan.launch.py | 99 ++--- .../turtlebot3_sim_rgbd_demo.launch.py | 117 ++++++ .../turtlebot3_sim_rgbd_scan_demo.launch.py | 119 ++++++ .../turtlebot3_sim_scan_demo.launch.py | 161 ++++++++ .../turtlebot4_sim_demo.launch.py} | 5 +- .../turtlebot4_slam.launch.py | 23 +- rtabmap_demos/package.xml | 2 +- rtabmap_demos/params/champ_nav2_params.yaml | 288 +++++++++++++ rtabmap_demos/params/isaac_nav2_params.yaml | 295 +++++++++++++ .../params/isaac_vslam_nav2_params.yaml | 295 +++++++++++++ .../params/turtlebot3_rgbd_nav2_params.yaml | 287 +++++++++++++ .../turtlebot3_rgbd_scan_nav2_params.yaml | 301 ++++++++++++++ .../params/turtlebot3_scan_nav2_params.yaml | 295 +++++++++++++ rtabmap_examples/CMakeLists.txt | 2 +- .../{launch => }/config/euroc_left.yaml | 0 .../{launch => }/config/euroc_right.yaml | 0 .../config/slam_D405x2_config.rviz | 0 .../config/slam_D405x3_config.rviz | 0 .../launch/euroc_datasets.launch.py | 4 +- rtabmap_examples/launch/k4a.launch.py | 9 +- .../launch/kinect_xbox_360.launch.py | 3 +- rtabmap_examples/launch/lidar3d.launch.py | 220 ++++++++++ .../launch/lidar3d_assemble.launch.py | 226 ++++++++++ .../launch/realsense_d435i_color.launch.py | 15 +- .../launch/realsense_d435i_infra.launch.py | 14 +- .../launch/realsense_d435i_stereo.launch.py | 14 +- .../launch/rtabmap_D405x2.launch.py | 2 +- .../launch/rtabmap_D405x3.launch.py | 2 +- rtabmap_examples/launch/vlp16.launch.py | 127 ------ rtabmap_examples/launch/vlp16_zed.launch.py | 121 ++++++ rtabmap_examples/launch/zed.launch.py | 6 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/README.md | 40 ++ rtabmap_launch/launch/rtabmap.launch.py | 12 +- rtabmap_launch/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- .../include/rtabmap_odom/OdometryROS.h | 11 +- rtabmap_odom/package.xml | 2 +- rtabmap_odom/src/OdometryROS.cpp | 132 ++++-- rtabmap_odom/src/nodelets/icp_odometry.cpp | 7 + rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 18 +- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 18 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- rtabmap_rviz_plugins/package.xml | 2 +- rtabmap_rviz_plugins/src/MapCloudDisplay.cpp | 3 +- rtabmap_slam/package.xml | 2 +- rtabmap_slam/src/CoreWrapper.cpp | 12 +- .../include/rtabmap_sync/SyncDiagnostic.h | 149 ++++--- .../include/rtabmap_sync/rgbd_sync.hpp | 1 + rtabmap_sync/package.xml | 2 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 6 +- .../src/impl/CommonDataSubscriberDepth.cpp | 32 ++ .../src/impl/CommonDataSubscriberOdom.cpp | 4 + .../src/impl/CommonDataSubscriberRGB.cpp | 32 ++ .../src/impl/CommonDataSubscriberRGBD.cpp | 20 + .../src/impl/CommonDataSubscriberRGBD2.cpp | 1 + .../src/impl/CommonDataSubscriberRGBD3.cpp | 1 + .../src/impl/CommonDataSubscriberRGBD4.cpp | 1 + .../src/impl/CommonDataSubscriberRGBD5.cpp | 1 + .../src/impl/CommonDataSubscriberRGBD6.cpp | 1 + .../src/impl/CommonDataSubscriberRGBDX.cpp | 1 + .../src/impl/CommonDataSubscriberScan.cpp | 24 ++ .../impl/CommonDataSubscriberSensorData.cpp | 20 +- .../src/impl/CommonDataSubscriberStereo.cpp | 4 + rtabmap_sync/src/nodelets/rgb_sync.cpp | 7 +- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 33 +- rtabmap_sync/src/nodelets/rgbdx_sync.cpp | 25 +- rtabmap_sync/src/nodelets/stereo_sync.cpp | 7 +- .../rtabmap_util/pointcloud_to_depthimage.hpp | 4 +- rtabmap_util/package.xml | 2 +- rtabmap_util/src/PointCloudAssemblerNode.cpp | 2 + .../src/PointCloudToDepthImageNode.cpp | 3 + .../src/nodelets/disparity_to_depth.cpp | 2 +- rtabmap_util/src/nodelets/imu_to_tf.cpp | 2 +- rtabmap_util/src/nodelets/lidar_deskewing.cpp | 7 +- .../src/nodelets/obstacles_detection.cpp | 2 +- .../src/nodelets/point_cloud_aggregator.cpp | 2 +- .../src/nodelets/point_cloud_assembler.cpp | 6 +- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 2 +- .../src/nodelets/point_cloud_xyzrgb.cpp | 2 +- .../src/nodelets/pointcloud_to_depthimage.cpp | 19 +- rtabmap_util/src/nodelets/rgbd_relay.cpp | 2 +- rtabmap_util/src/nodelets/rgbd_split.cpp | 2 +- rtabmap_viz/package.xml | 2 +- rtabmap_viz/src/GuiWrapper.cpp | 89 ++-- rtabmap_viz/src/PreferencesDialogROS.cpp | 20 +- 116 files changed, 6102 insertions(+), 575 deletions(-) create mode 100644 rtabmap_demos/README.md create mode 100644 rtabmap_demos/config/demo_robot_mapping.rviz create mode 100644 rtabmap_demos/config/find_object.ini create mode 100644 rtabmap_demos/data/books/4.png create mode 100644 rtabmap_demos/data/books/5.png create mode 100644 rtabmap_demos/data/books/6.png create mode 100644 rtabmap_demos/data/books/7.png create mode 100644 rtabmap_demos/data/books/8.png create mode 100644 rtabmap_demos/launch/champ/champ_sim_vslam.launch.py create mode 100644 rtabmap_demos/launch/champ/champ_vslam.launch.py create mode 100644 rtabmap_demos/launch/find_object_demo.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_slam2d.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_slam3d.launch.py create mode 100644 rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py create mode 100644 rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py create mode 100644 rtabmap_demos/launch/isaac/isaac_vslam.launch.py create mode 100644 rtabmap_demos/launch/multisession_mapping_demo.launch.py create mode 100644 rtabmap_demos/launch/robot_mapping_demo.launch.py create mode 100644 rtabmap_demos/launch/stereo_outdoor_demo.launch.py rename rtabmap_demos/launch/{ => turtlebot3}/turtlebot3_rgbd.launch.py (57%) rename rtabmap_demos/launch/{turtlebot3_rgbd_sync.launch.py => turtlebot3/turtlebot3_rgbd_scan.launch.py} (56%) rename rtabmap_demos/launch/{ => turtlebot3}/turtlebot3_scan.launch.py (54%) create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py rename rtabmap_demos/launch/{turtlebot4_ignition_demo.launch.py => turtlebot4/turtlebot4_sim_demo.launch.py} (95%) rename rtabmap_demos/launch/{ => turtlebot4}/turtlebot4_slam.launch.py (89%) create mode 100644 rtabmap_demos/params/champ_nav2_params.yaml create mode 100644 rtabmap_demos/params/isaac_nav2_params.yaml create mode 100644 rtabmap_demos/params/isaac_vslam_nav2_params.yaml create mode 100644 rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml create mode 100644 rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml create mode 100644 rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml rename rtabmap_examples/{launch => }/config/euroc_left.yaml (100%) rename rtabmap_examples/{launch => }/config/euroc_right.yaml (100%) rename rtabmap_examples/{launch => }/config/slam_D405x2_config.rviz (100%) rename rtabmap_examples/{launch => }/config/slam_D405x3_config.rviz (100%) create mode 100644 rtabmap_examples/launch/lidar3d.launch.py create mode 100644 rtabmap_examples/launch/lidar3d_assemble.launch.py delete mode 100644 rtabmap_examples/launch/vlp16.launch.py create mode 100644 rtabmap_examples/launch/vlp16_zed.launch.py create mode 100644 rtabmap_launch/README.md diff --git a/.gitignore b/.gitignore index 7feaafd2..5fb548e9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .pydevproject .settings +__pycache__ diff --git a/README.md b/README.md index b45ced39..66ffb809 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ rtabmap_ros =========== -RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2, however they are not all tested yet. 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 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)). #### CI Latest @@ -58,41 +58,24 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current # Usage -`rtabmap.launch` is also ported to ROS2 with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this: +* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder. +* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder. + +## Logging +To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info): ```bash -roslaunch zed_wrapper zed_no_tf.launch - -roslaunch rtabmap_ros rtabmap.launch \ - rtabmap_args:="--delete_db_on_start" \ - rgb_topic:=/zed/zed_node/rgb/image_rect_color \ - depth_topic:=/zed/zed_node/depth/depth_registered \ - camera_info_topic:=/zed/zed_node/rgb/camera_info \ - frame_id:=base_link \ - approx_sync:=false \ - wait_imu_to_init:=true \ - imu_topic:=/zed_node/imu/data - +export RCUTILS_LOGGING_USE_STDOUT=1 +export RCUTILS_LOGGING_BUFFERED_STREAM=1 +# Optional, but if you like colored logs: +export RCUTILS_COLORIZED_OUTPUT=1 ``` -The ROS2 equivalent is (with those [lines](https://github.com/stereolabs/zed-ros2-wrapper/blob/b512dce6ad4565f4770273995b147122e735ca0f/zed_wrapper/config/common.yaml#L58-L60) set to false to avoid TF conflicts): - +## 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: ```bash -ros2 launch zed_wrapper zed.launch.py - -ros2 launch rtabmap_launch rtabmap.launch.py \ - rtabmap_args:="--delete_db_on_start" \ - rgb_topic:=/zed/zed_node/rgb/image_rect_color \ - depth_topic:=/zed/zed_node/depth/depth_registered \ - camera_info_topic:=/zed/zed_node/rgb/camera_info \ - frame_id:=base_link \ - approx_sync:=false \ - wait_imu_to_init:=true \ - imu_topic:=/zed/zed_node/imu/data \ - qos:=1 \ - rviz:=true +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ``` -`qos` (Quality of Service) argument should match the published topics QoS (1=RELIABLE, 2=BEST EFFORT). ROS1 was always RELIABLE. # Installation @@ -121,39 +104,3 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release ``` -# Example with Turtlebot3 - -1. Launch Turtlebot3 simulator: - ```bash - export TURTLEBOT3_MODEL=waffle - ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py - - export TURTLEBOT3_MODEL=waffle - ros2 run turtlebot3_teleop teleop_keyboard - ``` - -2. Launch RTAB-Map: - ``` - ros2 launch rtabmap_demos turtlebot3_scan.launch.py - - # OR with rtabmap.launch.py - ros2 launch rtabmap_launch rtabmap.launch.py \ - visual_odometry:=false \ - frame_id:=base_footprint \ - subscribe_scan:=true depth:=false \ - approx_sync:=true \ - odom_topic:=/odom \ - scan_topic:=/scan \ - qos:=2 \ - args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \ - use_sim_time:=true \ - rviz:=true - ``` - -3. Launch navigation (`nav2_bringup` package should be installed): - ``` - ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True - ros2 launch nav2_bringup rviz_launch.py - ``` - -See [rtabmap_demos/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos/launch) and [rtabmap_examples/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) subfolders for some other ROS2 examples with turtlebot3 in simulation and a RGB-D camera. diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 6aa39f82..5bdce58c 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -2,7 +2,7 @@ rtabmap_conversions - 0.21.5 + 0.21.9 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 4bfe58d3..6f300a70 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -923,7 +923,8 @@ void cameraModelToROS( UASSERT(model.R().empty() || model.R().total() == 9); if(model.R().empty()) { - memset(camInfo.r.data(), 0.0, 9*sizeof(double)); + cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1); + memcpy(camInfo.r.data(), eye.data, 9*sizeof(double)); } else { @@ -934,6 +935,10 @@ void cameraModelToROS( if(model.P().empty()) { memset(camInfo.p.data(), 0.0, 12*sizeof(double)); + if(!model.K_raw().empty()) { + model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3)); + camInfo.p.back() = 1.0; + } } else { @@ -2169,7 +2174,7 @@ bool convertRGBDMsgs( rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform); if(localTransform.isNull()) { - UERROR("TF of received image %d at time %fs is not set!", i, stamp.seconds()); + UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds()); return false; } // sync with odometry stamp diff --git a/rtabmap_demos/CMakeLists.txt b/rtabmap_demos/CMakeLists.txt index 2e7ce52f..4c662b4c 100644 --- a/rtabmap_demos/CMakeLists.txt +++ b/rtabmap_demos/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_demos) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch config params data DESTINATION share/${PROJECT_NAME} ) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md new file mode 100644 index 00000000..8ab7a988 --- /dev/null +++ b/rtabmap_demos/README.md @@ -0,0 +1,101 @@ +# rtabmap_demos + +- [rtabmap_demos](#rtabmap-demos) + + [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) + + [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) + + [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) + + [Find-Object with SLAM](#find-object-with-slam) + + [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2--2d-lidar-and-rgb-d-slam) + + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) + + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) + + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2--2d-lidar-and-rgb-d-slam) + + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2--elevation-map-and-vslam) + + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--2d-lidar-and-rgb-d-slam) + + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-and-rgb-d-slam) + + [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-assembling-and-rgb-d-slam) + + [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) + + [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) + +### Outdoor Stereo VSLAM +``` +ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py +``` +![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23) + +### Indoor 2D LiDAR and RGB-D SLAM +``` +ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true +``` +![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 +``` +ros2 launch rtabmap_demos multisession_mapping_demo.launch.py +``` +![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b) + +### Find-Object with SLAM +``` +ros2 launch rtabmap_demos find_object_demo.launch.py +``` +![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 +``` +ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py +``` +![Peek 2024-11-29 12-19](https://github.com/user-attachments/assets/5914e34c-19f1-4b7c-b4df-2e7084946888) + +### Turtlebot3 Nav2 and 2D LiDAR SLAM +``` +ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py +``` +![Peek 2024-11-29 12-23](https://github.com/user-attachments/assets/e3c31c5a-5c46-4370-ad17-38c795db7917) + +### Turtlebot3 Nav2 and RGB-D SLAM +``` +ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py +``` +![Peek 2024-11-29 14-22](https://github.com/user-attachments/assets/5088be17-0875-42cc-b863-d14468c67f26) + +### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM +``` +ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py +``` +![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783) + +### Champ Quadruped Nav2, Elevation Map and VSLAM +``` +ros2 launch rtabmap_demos champ_sim_vslam.launch.py +``` +![Peek 2024-11-29 15-00](https://github.com/user-attachments/assets/d1a27c78-27bc-4901-82a7-59b5d24e6454) + +### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM +``` +ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py +``` +![Peek 2024-11-29 15-30](https://github.com/user-attachments/assets/c8f79b86-253e-4c8e-ac7a-c26584f43fa4) + +### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM +``` +ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py +``` +![Peek 2024-11-29 15-36](https://github.com/user-attachments/assets/a4b6e6ae-38ed-44da-bbfb-d3c30a301f9c) + +### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM +``` +ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py +``` +![Peek 2024-11-29 16-16](https://github.com/user-attachments/assets/b2235bd2-33d2-4c44-b6e9-9923a524632b) + +### Isaac Sim Nav2 and Stereo SLAM +``` +ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py +``` +![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) + +### Isaac Sim Nav2 and RGB-D VSLAM +``` +ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py stereo:=false vo:=rtabmap +``` +![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) \ No newline at end of file diff --git a/rtabmap_demos/config/demo_robot_mapping.rviz b/rtabmap_demos/config/demo_robot_mapping.rviz new file mode 100644 index 00000000..40e28170 --- /dev/null +++ b/rtabmap_demos/config/demo_robot_mapping.rviz @@ -0,0 +1,391 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 627 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: MapCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + az3_base_link: + Value: true + az3_odom: + Value: true + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + stereo_camera: + Value: true + stereo_camera_base: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + stereo_camera_base: + stereo_camera: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 237; 51; 59 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /jn0/base_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rtabmap_rviz_plugins/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Landmark: 0; 128; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapGraph + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Current Frame + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Feature Map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: VO + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.2877197265625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8703982830047607 + Target Frame: base_footprint + Value: Orbit (rviz) + Yaw: 3.7535834312438965 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006330000003efc0100000002fb0000000800540069006d0065010000000000000633000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c2000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1587 + X: 214 + Y: 77 diff --git a/rtabmap_demos/config/find_object.ini b/rtabmap_demos/config/find_object.ini new file mode 100644 index 00000000..515dbdf9 --- /dev/null +++ b/rtabmap_demos/config/find_object.ini @@ -0,0 +1,188 @@ +[General] +windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x4\x34\0\0\x1\x8e\0\0\x6\x9a\0\0\x4\x5\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5\0\0\0\0\0\0\0\0\a\x80\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5) +windowState=@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xda\0\0\x2'\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\x1\0\0\0\x16\0\0\x2'\0\0\0\xc4\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x1\x86\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x16\0\0\x1\x86\0\0\0\xa8\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x35\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x87\0\0\x2'\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0) + +[Camera] +1deviceId=0 +2imageWidth=640 +3imageHeight=480 +4imageRate=0 +5mediaPath= +6useTcpCamera=false +7IP=127.0.0.1 +8port=5000 +9queueSize=1 + +[Feature2D] +1Detector="5:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK;AGAST;KAZE;AKAZE;SuperPointTorch" +2Descriptor="2:Brief;ORB;SIFT;SURF;BRISK;FREAK;KAZE;AKAZE;LUCID;LATCH;DAISY;SuperPointTorch" +3MaxFeatures=0 +4Affine=false +5AffineCount=6 +6SubPix=false +7SubPixWinSize=3 +8SubPixIterations=30 +9SubPixEps=0.02 +AGAST_nonmaxSuppression=true +AGAST_threshold=10 +AKAZE_descriptorChannels=3 +AKAZE_descriptorSize=0 +AKAZE_nOctaveLayers=4 +AKAZE_nOctaves=4 +AKAZE_threshold=0.001 +BRISK_octaves=3 +BRISK_patternScale=1 +BRISK_thresh=30 +Brief_bytes=32 +DAISY_interpolation=true +DAISY_q_hist=8 +DAISY_q_radius=3 +DAISY_q_theta=8 +DAISY_radius=15 +DAISY_use_orientation=false +Dense_featureScaleLevels=1 +Dense_featureScaleMul=0.1 +Dense_initFeatureScale=1 +Dense_initImgBound=0 +Dense_initXyStep=6 +Dense_varyImgBoundWithScale=false +Dense_varyXyStepWithScale=true +FREAK_nOctaves=4 +FREAK_orientationNormalized=true +FREAK_patternScale=22 +FREAK_scaleNormalized=true +Fast_gpu=false +Fast_keypointsRatio=0.05 +Fast_maxNpoints=5000 +Fast_nonmaxSuppression=true +Fast_threshold=10 +GFTT_blockSize=3 +GFTT_k=0.04 +GFTT_maxCorners=1000 +GFTT_minDistance=1 +GFTT_qualityLevel=0.01 +GFTT_useHarrisDetector=false +KAZE_extended=false +KAZE_nOctaveLayers=4 +KAZE_nOctaves=4 +KAZE_threshold=0.001 +KAZE_upright=false +LATCH_bytes=32 +LATCH_half_ssd_size=3 +LATCH_rotationInvariance=true +LUCID_blur_kernel=2 +LUCID_kernel=1 +MSER_areaThreshold=1.01 +MSER_delta=5 +MSER_edgeBlurSize=5 +MSER_maxArea=14400 +MSER_maxEvolution=200 +MSER_maxVariation=0.25 +MSER_minArea=60 +MSER_minDiversity=0.2 +MSER_minMargin=0.003 +ORB_WTA_K=2 +ORB_blurForDescriptor=false +ORB_edgeThreshold=31 +ORB_firstLevel=0 +ORB_gpu=false +ORB_nFeatures=500 +ORB_nLevels=8 +ORB_patchSize=31 +ORB_scaleFactor=1.2 +ORB_scoreType=0 +SIFT_contrastThreshold=0.04 +SIFT_edgeThreshold=10 +SIFT_nOctaveLayers=3 +SIFT_nfeatures=0 +SIFT_rootSIFT=false +SIFT_sigma=1.6 +SURF_extended=true +SURF_gpu=false +SURF_hessianThreshold=600 +SURF_keypointsRatio=0.01 +SURF_nOctaveLayers=2 +SURF_nOctaves=4 +SURF_upright=false +Star_lineThresholdBinarized=8 +Star_lineThresholdProjected=10 +Star_maxSize=45 +Star_responseThreshold=30 +Star_suppressNonmaxSize=5 +SuperPointTorch_NMS=true +SuperPointTorch_NMS_radius=4 +SuperPointTorch_cuda=false +SuperPointTorch_modelPath= +SuperPointTorch_threshold=0.2 + +[%General] +autoPauseOnDetection=false +autoScreenshotPath= +autoScroll=true +autoStartCamera=false +autoUpdateObjects=true +controlsShown=false +debug=false +imageFormats=*.png *.jpg *.bmp *.tiff *.ppm +invertedSearch=true +mirrorView=false +multiDetection=false +multiDetectionRadius=30 +nextObjID=9 +port=0 +sendNoObjDetectedEvents=false +threads=1 +videoFormats=*.avi *.m4v *.mp4 +vocabularyFixed=false +vocabularyIncremental=false +vocabularyUpdateMinWords=2000 + +[Homography] +allCornersVisible=false +confidence=0.995 +homographyComputed=true +ignoreWhenAllInliers=false +maxIterations=2000 +method="1:LMEDS;RANSAC;RHO" +minAngle=50 +minimumInliers=10 +opticalFlow=false +opticalFlowEps=0.01 +opticalFlowIterations=30 +opticalFlowMaxLevel=3 +opticalFlowWinSize=16 +ransacReprojThr=5 +rectBorderWidth=4 + +[NearestNeighbor] +1Strategy="1:Linear;KDTree;KMeans;Composite;Autotuned;Lsh;BruteForce" +2Distance_type="0:EUCLIDEAN_L2;MANHATTAN_L1;MINKOWSKI;MAX;HIST_INTERSECT;HELLINGER;CHI_SQUARE_CS;KULLBACK_LEIBLER_KL;HAMMING" +3nndrRatioUsed=true +4nndrRatio=0.8 +5minDistanceUsed=false +6minDistance=1.6 +7ConvertBinToFloat=false +7search_checks=32 +8search_eps=0 +9search_sorted=true +Autotuned_build_weight=0.01 +Autotuned_memory_weight=0 +Autotuned_sample_fraction=0.1 +Autotuned_target_precision=0.8 +BruteForce_gpu=false +Composite_branching=32 +Composite_cb_index=0.2 +Composite_centers_init="0:RANDOM;GONZALES;KMEANSPP" +Composite_iterations=11 +Composite_trees=4 +KDTree_trees=4 +KMeans_branching=32 +KMeans_cb_index=0.2 +KMeans_centers_init="0:RANDOM;GONZALES;KMEANSPP" +KMeans_iterations=11 +Lsh_key_size=20 +Lsh_multi_probe_level=2 +Lsh_table_number=12 +search_checks=32 +search_eps=0 +search_sorted=true diff --git a/rtabmap_demos/data/books/4.png b/rtabmap_demos/data/books/4.png new file mode 100644 index 0000000000000000000000000000000000000000..5e5f9535b411a43ba62e5ce1b1fc03415109d2d7 GIT binary patch literal 23569 zcmXtA1yEJr*QL8bTDrTXJ0Fewq&uZc1f(1329X8Hc=XC3Yci5XfQA^m`aMWTHt3G_$onx1K+EcQQm~EZB%YAWsgRvI(k{)gv65SY}S@**LO4{iKt9 zy1C(f>xpSq#1Uz&{$W<=w4L(s^yT?9RV&R(FO6PeeM zu#dQrBd?hvilZSKMmpTda4a=CQi|W|v?z(9y?OAO1h-@hLTAwo_Sya}855xysk6>5 zo)YMTm5kMqg5LQCFYMyZl0d7-cZv0^OYX`CteOY~p?jlxOR&9o@HHM~L^^8|V?dwZ za5g@lctJAx5q@}(q$Kl>U|gt(_fiwT_~HknH3;3+4k3)~npT%Q9*MYbb0zxZK%e4L z-*`w@o|WE;Q@X*Nen)SF^XtmPM(DA8AubAnd5ta;LARI}X5NvvamU)JhwwrUHaBEY zI9Ns+KkLdYk(_x5d5X}3y+b?v)marm1s-7psv^(HM%#nG9Nf(ie2dklu76n!O>dl} zu%x^oxf&f_qvQPHwdb#u)0-g8y;OTRcfPDKyN2&6f3AHtT!`O<_H3AP5n~`V%&j;z z8CQY7HLhZ$j}fQSq7THvuPU;~`+`lGpd1lU8YZ5=vlp*R;+sOctv}#OK0qzQN_)a= z3>PHHMO=in1$B)-{3(mtgh9?!ES;~xMH~wEk(`k*TCQ+Xkq!>wZK&{!qIiyDgMKQc z*pNdc4j(NHytN6sBOp3{==rNb|9E{p&yWLOOFvTXB=@^CD(W}vGV@0q)?_l;oD`fL z!*Ji=-Uy#pndMUcQz2j8NoPXytCO&iQ?B!)-MSTEy=c$!8?eHpk3E0QAI8SS1lCB> zG&+i8t0@$O%4$~4lf}x{En-CU6lpwS)KAzod>OMYpABxM(Ys5a4x*RZ!Z#G3c+fSz zs4Vh{(z#Rd-+GAo#3YQO;F3Wx<6f0wYDv%n<2P7vX=F$?<3fcK_!VqcjV?x{Z&18b zQ}m0=cqMp>73(_G7Q!gm?hO|vfh(oz|Bq2=Q8Aq57>N>aIJ(S+ak}IKAdQzIs*t*!H(-KNig)bwe_Ct;&H9 zMn}bKr#-#FyTlX}SV$o_xA_`_2iHF0DBUt$0q{YROauiKTdcH+iY%$5BqSu1l<|G$ z+w4gKh%_*mDfrE4)mdWhlZ>^FP@8JaNLFh3TauW=Ep`qVlZ#L5A@W#jaNi`3$Ti;J z@U)XL2{coTU;4D=SoyMvQZmK#nRBJwKeb!wpilXq35A_Ka7j<~6xxQ3N zSsA;-*Lha^GS#Q-OOYne2%m0ZKKDnTEjAv%D{p9Ox;fkENli^vE5XM=nm+L0BV|v8 zj9I^(mbj5p(297RBI8Tm&gKeWo-RT~!1Jf)DLVPjq%!m_NPAPJ$hoXt;t5yy%qZe? zrs|@=FHrb#RcG~E>?a(o2vy)>Hp@sQ(V0ZB0MQNAMTJoUM3y6t{2?!9P zh0!FU6kn#}vbFzsS5Ef7t8;mKdwY5LtA`JrEY{Y4uaknBL0B!E+`sn8%U59t8Pup- zTwYixoj-Ibi3kt>+(+|Fx3*(&wh`dx1xF{fGqntZ%R0zvE*&iYbCIt;|CedRLL=tK z+yWKw+<&hR=L4QEbJHR8>w#&NS}dfoG}_hm^&S`7L(1tKZBKup!-83E<@%wG)qze; z4_lNkw?rMcVTV7@y&gZUJ*=)ht*rHJx#sPq>Tg#QY+B!yxV8A+oUAl7G-#Fa@$p^# zg*vONs~ePkQG@UgXJN`<6C=m%T{-n=oWm`i3Tfz<>>f$r6b^_Kb8#Ge9y*6Iw$QP2TXktDy&<>G%Gn8T*`H} zEk0Au9k;s>Z1HB-E%^q0BSXWlIXO8Q8P#m%vvw7gmEyj)T$w7Bm6cf7*g7n))_WLN zXY*z*l&W3E+kx768;gCLIZOZ;{{ z4ARQ$Gy?>Z6bgnTq|hL!I%2wtZ}{&#y8VI{>nbNFGTR$f7Yh=_>Ou-WB&vroRT zrmhaS?z?yIaE8`aS1Ywy68y`{%PIYDH&%XqG^j|^V#!po=E>X|i0?S>CreIF)<4|X z*Z`{zbaHg`yP8yZcz77kl>mA7z~g>2lNX$BT%Wn2R zhmyhhb2(I+bod|6RRB}budvp8SGV|hR5xBQx&QdsYNZGMB3}cg8H8c=Vl6L+x3)ymKs~ZpSyGH=zSrvN7YNllncl4Q#Cc zb~aHupNpG2amW&Y1S*`2FJC_QnU7iHcaTz2p58ok+>JRuKdrqy^kO^-<#GQUVzG!)7$+@QxcFX`Aoeg~`*bN!!ip zr$j|M92DQbi#GF>M!!6dmw?Cr{ri_~1VV?mwYBBk@$^^ZwAKCYVn?g&stcL&@glv$ z_x6lB;rwCk#oxlh;?t*x@t4Q(=sxoXeZR+B*A~OyJ8ogpQuY90{8~BL%MExQV$5Cc z2C>l=C(GVf`;+7Tf9n9;!ay46?*|BkkM!6>xLV${1mx-Y4{rnn#2?Rke8nY1 zM1IxRUxAfAo<&Hsf_)es9^T*IkHTjJr~CW&Z(v2>k55*9HPzPgh>2l-!mfp0wzjp2 z6>Hb%F0ZX&NAv)rY4f@Mb8>RByu3U!qum5OzV?ya7|VKtjfHjpB)+MsQ)MjXy4kxk zlHT!rT}3J8lM>!8i^{20H?xfJLxOVWp%~Bm6_PiMeZd#(VvaCXqw-k?XnVlR^Y7nk zP0*FKe;9GncIJc8*=@0Mx_Qk)2r{<#{XT7T8l!s7BedfQ3Je@L>+>Zzaq!^0Oj+hN zUUwH(_7zxJSw%!dz?PnGZkiWP-k$YfzZ~Q~mn}Nz zr(ORRIepwQdEPlYJNp-LuL>!FTUBY5IXF2zUz@yKhoKXZ#`br0b@lg$Ykk*XY6VV4 z#9=tNcbq7io%tp49tKAurmFV!Y z)k+o}=Ik0Ism@kEo%$Wyn`C8X2FZu%VI)4?A6CJiFFFtgwIE|t0NZ<6sIiEZFI;ij zGHNkA0WYFtX);wnq=>k^0ILJJ8YN5`7|i=7Xu|Ky%S)X~LnVgfGVMRp>Jn|Y{3AK(MeE$(~r^72kAKjlj03N+BdB-5V8O;XMYvlH+4|2#ga7!!h8k)QLg8& zY)ElCutz^WC1ky?wrht*h&K?3>lwKRO-4qN1V_ z5)yCTAU8;eiH#BbxKE0{%}eIoctLxT+OddQF6m9c)5M{axb_kIR*|sJP8hAMtQ;l{ zyGeXQ{=ci!5s*a*oeDAX8|6G*^aINit@Qz3V{ zveN&N__0wl^tB)U{JUXtaWf^CPAZov*r@fn0HY~PI$xFsRsq%-4Jia<34j0R+w(12 zT3Y=>NUb297=>vdPA_`UDI4b~FVZ+%N-@6>Lbo2EU#fyU3^y1f;vTGQ8 z0C7?wp5iqHMfj0gvDTz5K{SG{W`SH{kf}jz`cYZA`s?FqQ^(Ws!a~NDAP77lJOqGrA)MXrwllo; zIJQ;~b?il)$%8QHDNE~g|L4UoUSwF%I>%r*`hrJ-aRN#N`NAO4SIrSMq{TB=S65li z?ROw855!a2S11=xQ9hqVye(177WsHWwszZZlk3-5TYGU(k_)oK$6qUU4L{r4PXvEM zK}O34SrTN5G%J2mOcxdLT(FLFiN_uZszeQz)cUzahrh$AtRR$SWM}(r#lCR@$px5% zPuugP0wsUem|KfsnKnq4b52du2Uozu<0&Pu=0A9NT&=c!syqbutNQVSjgvEdWXD%L ze!#Bc@Y<(Ld)SiawEf`(M2COTvq78XX)oY;4=h)|Vh-e&r6qkq^51)xedaZW95SLf z$bS;@Eu(+5YYT8*4VY~~qAg3d#V<$`r9x#Cil+~n*8IuRoEVa$V| zFV{9V-;O!^UxGsikxxBGv<2Y4zkfZM2!^fefU&=Se*j&3{yR0L#ggiCSkd%z<>Y*8 z03@&aU%#St^%#0Pb3gk6T6Kkpgg*7~x(5#P4LL#Q8kQ^Ea z(>l}jkFFRt4!3b|@R^Wew6NeU!DCMm(pZM%p?Br4(8AQRh4}>pJY8I1c0l0BCkC$@ zs;mEDMc_5Z$BvKheSGxo+p4Q!4ux{ZO@4a32ag7FL5rc2z5U@(GGkU&)~wwJOG`^9 zCnqnj4DM9(n%k9A|C^bzx4>XQdIuSJ4GPW5$>AVC1E%&RCkLF^!taHJg^wK%E3^u6 zzK`bv0E4b)fIK`oImxP1dG+ulU&t*W@VB4LdHeECm+kMIfpb{T25^+ixe5a)6xwFY znLK3J?t9lAh7KZ0m~;>jpsTy=quucN>j@gms;Rf0A@kx z(_e>Tt+M&UJ70=4-5Oor605=Li> zdgCqPdh11)Yc6Ta+9tN8bL4M5wAI_9y55=amqPs-8|UtK$Fd||?#AJRIuE8xl85M| z*V>GoeSCy*P%P{8mWRw4cbj1p5FTt9051oC&Faq&hs!PQft=}yY9*Iu9$G6%7MI49|qWOtiX}yPjIyhHbBhCXH z*%N`C7!F+0e5rZ*AO%t^Nd-PLQ#SF?GDxUAGj#$}od0BcB5teGgO1J{s{ACC-L;n_ zq-#!h-8#eS`<9DX*3#V4vPgp|CN8evtC}^BB#~QwfaO7ru7jJ~lzsmTneZ`z|40t`^a1Ce4cbS2^;uhMQHW8lbg2e*ug*GBVUGt_<7 zeMTNG5AzKa3xEIsA(v`T(I2EVMZ%ikDGIjagpr=^?c7^x!ssp?n@pH}s;n`eoQN~n zrsdNc`v!ec3gm!13He&Vu3$gF54yErj3Cl1xstnmPZ5T{J#*}=Wt(A<J(ch}gMlpv1ZH&K?(VKe(1cS{u0#Nqt^!S>R+$Iz`zkr+hW7S$P#4-Y zOdWWD0?d7P6d-X>l_72^0boL=3X(Q@%GNUoETAerK0eOQ z&d$%z2lNAM!xbi$f(3i;#}?Tr8x#_R+Z5aNbu2N>Ga7F4DA|iUUp`X5r|bE;MK&%{ ztO!t)RObQu0o0V%btF-;2}DXI(_zxo6!Dbg?lf zJP3%$US5KvvHEHMRm}rVw^&Q6Z5$jqFgH-{gDA$x$OxbxAXh-m`S*y#zp|ln)(&9E zSb18)Xlh1Hj`YK8VK+l;3ek+5oYj>T#Mhz#`hc_rKK$3OUw}?aRFo~8^blS-@DLCX z0H0_E0oAXgw|9$i6|^z7y5{gFIB811t3*fWK-_Bs6HxF}1z^5m!a@00PJl#T@$T^I zU>aO~=D4oB!=CROhV7{WoT4PrMr)0$kl8nR9nS@!MFBhbw26Zg6F7E|Qi)>j@Qgw$ zy>l<}yLJ$h9R%`u0TRj}8>Vr|e1qib(EiINjN-wz98GGKq_sX>GTwPs3 zFtW1R0XIfRM+ayVRI#9l6Y;*-o|%~eTl4(v&qVN9z7Q<-6958Djg8$KXMlxUSRf8= z2N~bM$O!P!fQm4#lJL8)pR{cSwf~SMSSm=*0I9eL`W{Rb|0B3YIvz(JA4R8-!3Lar z`2yhJ=os6tOp_Sc;*crGfr-%n)%YS}o9F^zs`9M-z3TZuPU%ZR@!K>j0IFRCW3=&7 zT4a$*L8UV~K5o+Lh5Nh()(7yI=l#`ziy&}FRg0n{ux5&_TP zU`%rM78}L5xm^LXu(GlOC?}A6ZTZm1ENyLV0r+@WSz6M9jDhlLe7rD20xt?A<@57% zb@4k(8=HYk(Z|R2_4UO%n{*DNZadt`8b`AJEDs90-lD#1JW7b}d37)jr7SnrRfGz` z@Pl_yu?GJoIQq;?lJP6wZZ&`jO!V|XIR;WN$ejbMz@XSh?3$nxKNlT5&Hi1~V1wO+ z23#2!7}Rn^Nn+*C{{Tb{fV^F)S_!X!0AP}65(lm8%CuRs)j*cAu3K~vn4h1o0wDAB z2Jl7zXO51lh*GFCB;TB_0k*5%|LNYosx58EGQ+c%HAU;dLzn=~(ZK;2Cna^6euXFC zA~rV6nm+?a2X!#@wwEXtV}Q4}H>en7xp;XCQ-FU1t`-M1kJ0J}^-N%*F8`sjFAD-r zuR;F!BwPVj=6^mD{UyVb`Z_GB&?aZvENiG|$J+Rg8!*R@ABpbI0QT0Hfs_FjMzCKo z1kl6(Y7!xLq%!o8+O)K^&GXx%BL{|LR0KF+NQZ}q!0(t0NM65ga0G=zD`05>wzRb! ztYwqsJud-$2ep!X3LJ{(VV!ojr~413^wqb!yVl~lj4^Fy48gzo?R`sIrcFrt@- zfER4n<-WeYdE-x*RP7!INsiDcS!sJr&G4_H(rf^vNWK2jcctyij}iziKM(rCG*U}G zASDXP6c^aWm@)mXOv8{0c`f8tq+cO@E?cOndswXmLNiM$fV?1#ut7qEU5(y=0@a** zfh%oLEiOLZrcQ6b6)y_m%brFNoKxU;SvDa8G^tC(kS;*d_OOiYUE<^84_YlYf^zzI zu7n92A@DYi^l16Qe2vmYhk_#9EVPIo=eFy)(NT-kJtj-vF8IoqX`jRrY=4{q?vpNYD^n{HG>a&&Si6jgW&Q4DphVM|8(Ny6fyPU z*DfBFy)BVEjoa$oa$QD}4_cmJAPD>M1P~03qhnm%gQjg7BgZ7m^?%4 zT?iH$1=Hoi5?z9sY0>mSG!bXo$PUN@_;ENXGob@Z-?E{2W_Z3bg0)Q95A)x6}?$7K{ z94^%yMK*wY6)}`Y2Q+DdX;j$`HgSR%$OiZJCkxY}PL~RXWQ++s^h0UcHH!yc->q6j zIeg=jzcRH%yz$k;tQ5FIY@sc{-;F2b^G5kku#xLaF@0a&o^rt@NiCCC*WU$Gm_95H zO~{9_oy;606nf=Q*jvjrf4Ug9Lsd6pbXvMck!FqoR}6I)7boqLtf(qN&C6a7ua(^}w_BaZZ@ zTBeKUid0`}SV6yZEBSvdfD;i(fa0tM(>7)!nez7(CY^6L+L?*ujgBC~=vN?hFoI?X z-TZ+^wJjTJmedLC)8&)}eV&vsAXlt@uUn(ww-Om*4JnK9C{^H$Qj zyy<_Sv##xd5R7p5ZA$Es`b70dR4aNPU9+L< z2fsBcMD_yaKsXGI@$G|@K^FZ2*62t^mvc^JEW{A06oKQTBaq?A^Bt0f1{L4hNsbGEJRhI9qz?oxih`Kpt#dPlTYF$hxv}&sq-fQ#dpDIustt6n3pGm z!Rvh^Q@%6%Mn=Nvw#ZDLGwRR43OTs*FrxRYZ)YH3@u$>_04@rDyt%n)Y6A2&M_!Dq z@Ml}$U9E5D$OJHTy3C08>$UlJ6Qxo-DPIUJquY{a;lBB4T_B6-ObRM#o7G(*5B_TA zH6@(^Eoq8+D^bn-PHaxw+}aqj&L713JTsv&^IEnn#Tw6I{QLm%0T|;NMdV)5OwuaH zj%jRJ3wmbMDI=%2>+nQ0&x27037J%q1>{UMZ0Jv`vGVZ06C5_^6*e=q1qklx3>-)O za1c&=qdQ^K_HDz(drZ7}yKD30*y5MJA-zV5xb+w{#85y0J$-z9+}!Zu_)b8nRmvuJDC#elK7p60szF#r5goCD zxF5oPhG1u1%VVI+R3jzaiy%pzD#0(5hLD?EwTpjm4ujzmgI-I}9r&tWf8mL<0qq4z z2&N7KdC{@c=5i~a)i*GBc)T1BIFB~TgNEqi61CuDNNQ98Q+I2D6ywFpYr>i9fgaM3 zB~NL5u;%tp4xgKe7ECk5IoNJngiGEGJT7`t)2W352MmYLZB*1Ke;9g?yTsku?^idF zs`CiOyFP8e2QqZZBn;&o%uV#yy(F(7 z#a*aqr(C=vK~{|>G+0We<8;F-&SW&NnR-Xprr(X*50^)0UPJtcQlmn%;vKaBjHI9# z#4G*JS*s2tU&2i@+9h{^VusdELD5`0CT=URioS{pHp~URGh)e0<13$N(QcklN$90z z<^-EnG#)qh5Mde@cSB?a%k&$}LJreEn1-6xCCEL&GcV01B=*F0pR~#dtSQX5x=a%s z5H>Atzt*mp1nl(0K9--7m2K0{PN2*&I3Ne_{G}pq4lprH`aT|^91PEgQl^NI5bFgs z^F7-e-j`UGudv7pOfbYlB<+cu>ZX&tbs4{$aQ&R51D$!TSl7)H;zi%kWp{Ryd)ZNB z?tg%fCC+{PB}Yn01crp4#Zc5%6oKWIQ3(O`v=SAm)Ub$ah;c0C;v<}k77}&Sm#LcM z{gY%nRrUHulbw4xsGeIS~i{snyH8m%X>J#TX6CIa{oW~PLqN7zOn51Cf zoO(9PSI6jTfz=!bcvMXdWO8uXH9!UooI=&%0gXgumrZ5i2nB3Bz)qPe1@Yzchum)q z`j%Du-N@OGOxi~o<9>8WyZSK|Z+xE2OURhmBRI@atdjo5EpHiJgr5WI1JEM6xAr8| zGS&*6I@xx<&JRs;lwOTTW)7EOp4 z>Q+wgMrI7>6f)ieaXGIYs!CAXku8kEH>;tYWsh{_}O-VIMJe=~pgtLDKw$cPK;Tv2XysBk6)*QK& zt&A$@Nei^Ez#<827Ujl`W%60+@=3I#s@H`sl$5A9jc<-ouWs8}d0RdQl-~-_GS@g!&p6QFB0HqyxL>hhKEtviE z8s1`j?q0tnn;nBPqbN1yq6J@!I1FJH{3EgoTw$|zzUnl}lcKR+KHxNDaBEOLZQ&HQBq!j;RDhHZ#eih#;9QtQ)3 zk)*H4V_(7jdKT}=T~QQ4t|Z@CX$i%cvT?a9CFU_omJxxr7k%bK$6Cn{2Z<5MiIk@- zSvEG2Eb7~8XROO4t}eN@ou|Y*4A?}+GzdjNYJaNsVBD6AAiQ%L!YeLRnzT3fD095C zJ$}3?L^!Zp|IL> ztw;w1j?>j_lD(t5dvYJ|O_MrWND>h&FvPw$Pm0a!x-z{^SQa`KMcSzMZN8r@OOEi)7Cl(uRkz4 za!c0}&XVqO=oNvs5g0g13yTGW^21JUQ3Q$NU(2Mjh85}_UxmV)D10n-geu~`V|nG+ z&qmz+UeAOevABpve4_65iWd+d8+LVgJMapJKjt=S zTj;&2h8gxKdb&pPN6P1NcJp;{-CySS@YEdaXsA>l_CX(yBEIky`G07DzWZ>bZ@+Nb zJkWS-*Wd$~zD0WEAk78oa4P@8m=6f+_jDONVT9teVHk`-rl{%3wQA@Ak|ZW8`1lYu zGXwQ%Jn#Tq zOJh^hWFWi&K5w8du{^a*G0Tty)smUwI^H)(MqNZ%nxWj=FlN{H6C-WPmWqS67S*lm z*y_FO+Xg#zz6b?KJ2Ae+goS)XjVb*!nLin^_RmGJf8U*)RL+rJ47+{>y+_Twn9|U? z@U4E)ar-7H;Aufa(2k#qHfxN=o2~F(JfH4Hk?ziO`&aYpsdhfcdd;sfZ(#>6GR4m+ zh(}Gau0$@8#oyg`7(TKG*=H^Z;w4)8!h}5w$RWhA>UH%)lnHnj8i53Z`xP@WM!`4* zwHdiXu+m(bRo9#FYB!aYjw*qe@&?DO*PTiBB>7!mqPRG8molfCrJ3SqQ}mhi{m+oZ zoRoimx=xf>ltW`3CZS~5#%mxpgSQ+U=1?x0HGKPDQP^6@IVWhrfEyU$;~aVZBA(v* z=~Sk@w6@md_z5t0S!3qX@4;&*R0bV`RFHkdj*nE5`tg-|y))}ZO|m%M@h6%&WhU?L zmujS*)kASEx3@=_-F~{|h#hKJ5~4R5mPUhg)ep+t}$ZeN{ARj{RoDu!TU50RzN}K-LP#qR~`P--3p4u^yn9|7nmueDg|;qEYr? zbk*&z!N$O{t&!u$7owul$Gys%NUx?zp_VwmRAERS+3B2E=XLx}z&z`i`0Ilgvb*C_ zQTzK-7Bj^AG}~w@(esTht05Wj{sP?YmF_}vK$3!sl(f70;}zXfV=OK1gfU2rfK{FF z{-?@xI!bzY{`^QN9`c+xRqM)zj6A9}oh>U_|!;|>goA*u|c<1bQ(DKn|N>0>yW!4{j;YsP?CdOlf zA&8X+^ujPqp4|UR%wSE{Ymyb`Bi`g8)5bUbj(OxjwO}gT4-nEf=fQUvk_g51Dgm&_ zT==>o%uPM1%2SQ1d&JV872*CXp9SPuNJvO__G>Nn0*VrW<{ZQ0+lQFb$$Uo`JS7byq1?JFj~#KP7R$fYs&&4+AgDa@uw4qD=0<9=^E?_XSp2)o~S) zkE(Cr{vg|8b>^SPy!vhar`=Yu^j*0I%$cx~WV?0-hPv%WoT(YzBjD>`ouC40g^F}| z84Rqw-{373seu)`Oe zN|K5qnTJ7A_pgBbog}@auT(gbMOHpXT3Gsz5+t4lUu&oUURXIurNa0(#k|KBEVP#I?XEtbU1dPrG}G>9u(btZWkIJPu_c_RJIwlN%Nj`cpe&( zZW$d|Dx5MkYC~mt$@z5#jqb2fyr140%$MX!pL{ms#~1GWt}t_ z!<$u#Ar>^sKt-3G{eip$bk}=Wi(#Jd8P!Zz@4C8Rc{Vb{6y34l#`s%sL=)!H3CtsT z0&&kokFno2(bG92vckGfbp=A=y@S3Hgy~m4j?&XeljRqqemDH8^3R9v4)^&-Q+C;?Likgn-XT2G>}Ps zTs0`u4(^T|*b?P}%}Jmq^*&1oJh<=6v?9(6N9@EiLrvqU=+P=@{aQ)=s$pg}CyGz8 zrVx3WIp<*c9b_X$-f>ENN)6tn#)*oS3JIxYHamxFgoNhzF7PYo&}WzyBn0iZ z*F*m`-L%Sjn{n2xqV}0 zgPN$9gZb6_=1&ZC8(+Q>f53*5jzDaX3z#6i!alQsFxr_?k}+$V^fn_7QleqLLXQz* zP3E~QBRrU(zN@LJ30jiBWc&@fl<$-W@Ohj084-9c|U6|fM=9TtZOrlK}+E+Y6 z)ye-h$!cD^yxiUj^4_JP+}uZIrNqLAc17!k^9mL$Zirv!72ZO;i$mM&vA?v^F*)Fc zBM1BO35aeH^R~zuA%-Eq$+ls`V^0ax$u$J>c;*TG<$KV3%}W*XC3$L;_nP&HqLB@q z*(~PWUlsQ9aoTSv<~J@hi7u+;E$pJU;~rkM248iB49m8^{J}Le@eya-2_e8raS#vs zrg8_H#06Bn`V}m^Td9pW`Qzr!<|)g0u>e*$aBt+me>0E#3Jc@vO;EI4-HT94#%?uS z%4e``NWZ1eAU<9#nl20NeRK2!dQ3r1{-dG-d=%)Z%aBPW8HDV8!%1r88@He?qT{he z>(Fv>PtDnD&}Yy1=;Bx~$t1o-U85W0YHdYYP~-ldWZIvxX_BHrC0IR5n!;GaXt)%O z6c@wgPQ0{Bm!yqE)}7>{D=7~Vo+b!&Aw$^)&@GkH4xY_-D7KHk2rb-E8}xQ#DDA^3bG z9%h5l=CDIXq4Exz6%a`SX&cjq&NopQA8Qq7U!m$>7wFQixcJZb(jYFU$um@R!|Jqh%;0WsCCAx{=Wrmp?5 z*?e_K`9&rPN&J>&^^CJmY`r|C+jR!?K5|?`a5v=(Qa;ZUx(0~C_T_3s{jf7#POn$$ zcawWj==30TXKY0AKWm~Q8b+fD=P@E>6KClaE=xA0;R&)OA(~}0x_<(B%()3FNZ+K* zVzyt!iiabm?-)xYCyuU(jSVE896FH0EUTNn*>P^}=0{bfINUjO$S&Ejjh z21k0yw{N$es|;l(aEoisl9UONiBKeQ6lNFux< z&~4R4Cd9r@0m%zY`fvfv+(s0io(Z~;&4^nnSE>zZmK2OwcGI0mS=jcIe0V2aK8!U& zn{&vBA{sUS{y6h3Wm=7-3-W7@B{~AC(D%h!-p`NTba+yMq|%bZZ75|WATM^RGE*#pAxPatIEHH}s<@Qe2XwBz{N&w}Jnq`28)kp% zdl7K)u_6*tC9wmo^hV#aZrno(4gU0V{;Hn?+J54{ko_$$YK(-Qep4+AwlJ&R1FBE^ z=+*XbAtTOI1EP}yYF`%_$kA_?zI}az7bWWdNIc+{&*SXjF~jRV?XdB63oT3tQwgKs7dq>Bp-P z^Bs=bfSf5f1vW0Xc1+SMf$DR+%ya22HD?%iFWK9I#-KBIuvs=>*;2}3SKzyTZP(FARR{b&deG$;jK>J4`~&rJLsw#~E7z|mC{CqHW(ms0UX%PZ zN_wPqPQ5k4F;WXg`o;|RHCYLxfocS73^FpZpj2p|vKBp11O^TjY-v^=*6K&JIQhiw ze+ZfCHxYua6IlI<0&?qe8PAihs(q=iUa@{lFK+< z_p*Zw!_5R;MTs) zO&U#GsHb=?P_Z{}2?9Mhgqbb8$3`y(YRh-B+V*ek_Mma)=_!yqZeMY_+;aK+a0@-JRy3qx8%T zOqf34lcohla*!m9jXJC@pJv8JzOwQT313Ks6p;7#T#DYl;j)uT1dw*XIY`;Io7*eq zm}+DAJ%X1-n=w!hJ%fq^%lS1aDVVPTNfSse!Q733p57gh7y0cBzgiE`q~t+{y)b7B zk!CGeikls{62oM zv8|@Y^?0cPaBF{Nn7`V^rgVY97voRI4cFJ#V`HisF#^-*Y>8JzF(er_s1wjlL3Vau zRyh4k?PB#ouMpLrkN5$V4f0Anbf}LIG>%maMsm}sZ>N-Ml;pyOZo%?RP`Q+>5u{tL za0I$27IYPPP3v!%c`znmuv7bO3WVO#POk{HsDUGWk=2!JkytJNsl*QCu@!Jm&9L;C2iPa6>J&`L*ibk9m|e_wRi zOt2kEsp|PESBjL;Q%)UB{&pRM6U_Dg2h`7PSk71u8=wCYjHrx$FuGY^rvd=88S@=a z|GYC0NyWzl{e8DA97pod_U7j1+}vCQm_`FS8K7ROso^G#faN~K?%vuFg;D40KvKyM z=KNh4%IS;0eX%Q&Q^HFVulOYbgr%d8k zc`z_!U;q7IfY+)o3A>SWo7SPPU8}3@zeh}lVIh=uRU@#`T$OKWqpSJDRZ*EoQX$F$ znV-z5V#&0gk;Nw?tiU~d3Z^eLP*ErmLb|}b<%}UMHIf8#Dr5sprc^Yxwu*9d zchS)v>M$#kD{bKoPpPC(gnt-Z6#vohh(A*R`+al+U2qo>A1PG}Wo9IfcpV1V>bz)Yshk)1d|pgsIO29?wJ_;{|k-IvXe`DEftd6DNaz6#!I^V7x&B#Cyh^q1OpM&0LkS`YRud@^D_aIfKC~ zFc{OE~KFX26Y=jKfjqyi6nup(-BG`D3?N$ zMc5`r{Ehwx{5L4p+Lc^Hd@qLpbM}-5lQtV+$IH`cuBdnIq63gV71D$&d-PUICke0~ zgQjvfStF{4PY%&PHR~e?`1u-=WE{^cfaD$R!57=MG|Vk3?uL)1!;2ak8Ups!-fql9 zaApwW1Ej=}4M5lZoqRWheuL)+n9&DggH$-3w^B>m`iAh28~EZ{*;6W;0_zwf?U3AJ zR!Vvuq%tig05U|;CzTfoQmK);205P_dD|$ftou^p5?nbVFjn{7TTS$(flkgQ?Qf5&K%s7}?x@w*9PmY|i zP9bh{vQ@0$XSvQA`(~x(U#5+QnJg(e{+0I(*e-|`bW+2-73-M@>}f2MTbaK}7e0+y zyE{8`lE(fX9R(Xfeo>J^N=BBIks;Z^X5h83|4mL_E6aBUh&kB^Wt_2? zWZUeBn(7bbqEcT=BcPf;$rmt#Etv)#)cD7JF!BPlsQ;V$;c5b`xQ1(CuL=wMCT<=) z4aRZ03-j;}kHa#MfrDwJhnrK2|Lfqq!>N4#IPM(l*v>&l2$@;O%sg@IkzFz)J7k1V zB;(k7%ie^9q*7+KG9n`#q3BCRk(K&=`aSpv>|fX9Fw@Cz#cP!FgdDPGb~&)B+sBNh zi{MTi=>%P>I(9V$NfZXgZdNg81xe>?IwKdgRnVkU2%;t*un$nKIjSg2PS@*y`mV@` zQpX_F)iuHvv+v1%pF1db^V!DBi&bXWiQL>=G&?L*cm;rF;Q~?|>ff%eu7bIv|5``E z+fZ&#NlKa%^jX^6gCYAjVwaEAEF|}4by+T*VN}8>!o6@en*gD1uD$-A&Eg>rIA6kW z+~U0BV#|2+cOFZR$u7;;PE;t)C72RCduA<>dwhbEez;81!ODBnVfN&B|JZei6{(J* z$A64!^J5%=Vr9)V6t&pSW*Q0}m$PN}sx5HQP67n5e;h_Dv~8#wh%1pil?XTqWPea> zkB!BbL8~|>CdTbe@zS$EPcJWEM5gm!+4cbPE-j5o9jusif3y42?0i==gGlawh`9gq zr9#_1UYqp(x%bU~oaBK~4e4s^P^Qaz*mNvAip=-ud;_{V$)S~xf_y>en{M%5>0|9? z>jLeOLS>qw3vzNSvQA;V#tOWsQML3aV}HE!u-64))*ZXi!V(K%Wm=lyd(pET zBuK%$QFFe~$?yA8UF1S14#nj8@JE088Dg;+TwctSUiH!=_p(01?Equ%_|#VWKjf_Z zlLSksNzSP(-C%vjwT&e2M)Gsb3BEWZ8$0=IrC-5AsocZ|O2fb=L+c)mzEU=17kTIB z;cuu|LZKBlJL!D>>W@c&pgRTxFmuhK&fVlG!`R2YI*hNBb0+qFmNg#LwB(Vny{UOR zj_X{}3PX@vmD&7<4+O<~v<03E(mcDHd(XTVVJ_A4t2BQf6R)}a4Y2;p6cuh%!wj4; zHCf2mz2lSXkEwTUav;5-j_HLEQJ6G8!>GnwnA!IxF`z!AIthIplR5ger!C+O=OKMS zWYfHTN&@2?M_Tts--G^mCiurAZmuY?OL3>Z86SrlDESywdU5`)z-EMf+Di-fraas` zR#LM{D*~1vZ?#+O@zm?dBD_DL?yl4dc%4q4G!ce^ny)VZN*F+}K-`c}B3FwQUh2iy z+tTx#eWC1H%GNv>D)#PE0*&v$|}tBIo3oQs3*hN((~~G z9F*(*mn|vBrD7-S(r95akk0Tn=mUV>!NF`Y&T0r<9Z&T6*0Bk>Ie#^knFu#+2c?uu z{VIoR77s3Mom#+gr`mj;VDZWs+PKfY|JH1TS#$o{4b(;7;tQ1*Cwee3GsRSDBqlt} zE_cGq^M55~S;*U5s@je;X}P#UegnJD!@@JpuS+;N2{Y`A$uW1m7R8Arp9Qmr*lm!7 zzuMuGijI0Uv-JaGlgjkdNeBK>y^u3?q-vBO5WYay^ex zD-;x)rf5!5GAzc~A-e-sXO#ni~4+F><}hlTO=)9kM+1;_4Ah zikx#o%cSK@y|FHnJhF{}0E}hpvne+AtN5f`|H!0*4xhNjLwk9sfv*KkMmA*dnx!;& zvRA?~7J2K3j7x|2krCSq@WMb9$g5C|y!cL{OG`O6g_mEgXRcJXz<%L5KXvdg6GCy` zjy|nS3Hiz^t9y^FlN*)`D4dP64m+;Bg`u^npEc}-R6FHx2v<>|eVFHGBEsttWPmUj zAETzE_FL?KRMnY*qeKiUhs8~n%NGj!X)#FH;u(ceDXv1~4AJ${lay@F5#}U~(W6q! zC7v=&tlH=w_^i|>k0sxWIwEm?069xhMKavLR#7WxBq*HYUu>6Rgt9&@9i8KtXoHwR z+c~bw*3n`=C4M>Vocmr=nN2j%T*)|!qOH`DAz)ZP)tFKHvk{aX@cQM1R>NDWAD~`M zAMk68ja21p4DQLrPb}|gjAivG3_VVDU(aVg-yVIPR>@DFw&k65Uje>>iZM3Wg`V?G zKR;y>PQPZF?Kw?}?T9euVRT|YrR7kfyGfM%%S!!{biSEN&+5SWC_TkLX-D0s_Lkn> ztksJmkhEB}%zWQ>dUOSDnf>E00R{3(i9F0t7ymiqtlbHr1^`q6L zP<(_mdaWCMER>%p#^OsC zt~_v7AMMxmnpD*eS*A_HWPd(7Gq<~c~m*pug^D(xrs#GsHnO_OOse10T{@fN!kcl=kfMIpM2(;pXO2K|+@HFk0 z$vc!lT0xH?Vlq$k6Jz)%Y&*gheI#a?RzA(dswGcCh618Z1@W6I&mMaE0Rgh^3!@@7 z4L#H$#?FRi6drQ;JXwxpi*r?3kz)de>Yi$+Bg5RsF6o!G()oJGa=72@9Wgz{i4D^? z>4NMEl1<&eiYcxCzhq=(qb(F&sM0lw&&}#oZvPVFNH#JuDz`;11uk2=AhE^l44T)d zB6_8OkWG^O#0C7mef*(0^fF zXK~|w$Ifz3lvy3@@CmP09ytv{tv*q#j=L?Zlp(&7c=GKQu-Wh+1?cxl>wLqiLY3JU z;9I#UI`Ko`ALu&#d>^QiwAH4~8$Lc)!5IRQ1aW3+;?e9Acv|_~JzjhOq6+|8e|#DR z8wg7#p>`e28?E!5VaLDz{n$(X_+Q%LSLSKxHi|<|Np2w;E+O;M)nWs}IBX9)On1 z!b~FfHaI`tzGeO}8mGE;Ivj%{1JLT9xWfKL=+253W;OZrd6?ytM7&MYfcXYwlN))2 zl_ijj%sdPa-1~24etsVIxbx@tcLvY={d~R)OcOFPGFW1i>nY|)H1y~j|8c1Q3?T;x z;0zdq%&_ZV1(+A029vDc8o>J!T+=*ef?FE}Ws;!BOOL|z*}!Qtq4koui|s{?@Vfzsv-)@>R9bYh2}6L=eJh({o* zv9+@+Dl9B4Dk6We+79xZ_I7}W;1|N_(tGj4+?-nN1>o|?z5&b$h7SpGaTxqOZMyyU z&k39w0j&4-_J*$!o;+n=ya7tQd-u%De7@o5);G9BO+N5ts0S9w=L?%Drth{8ty(VR z>MHkRJjAecx(sDVM!}Oq>qbKSr^7qNOe~Q8{{H?o|`qFNTTQqCv` zc>$9h9DMN4Z4`D4yiuxl1+2cqpX>i#+_-NA1$ixkIOMUI=k@EBAB2PuGfwWD>xNDBhU%%rvMeqNRLd^#`LothOK_+bV{VkAcm%I-%zH3akbHSi zzW5a^X64q6I7nE5fS=;b4Qh~1_tN3O^y{dNC1p`Z@fqKj@`2(@z4*frQ>&_K5ZF4y$nq z?w=k8QXyEL6bn4-Qm}Ls6|L$lOokCoRN`d%b(iE&mgeRN`#LyL zhl)?h3S5Y4rI#MVnNzkgbeQZJgf(4Mki^8prV;%YE6O$#Vj-@ne1$Sr5-UvCP>~oJ zwrZ;fgIi{5APw!VTmicP5U{h>@EwbZi#z)2ik*VgeIta5B5Gqm^FD9NJNDD3fpk*q za zsIpyeC~a$98w{v2Jhb!ZCB7bk)h%@p`1*118 zV8!%qZEXSQxRL?5I(Mq1m{_4oh`*N?tu#vSQ9UKHv5_G0*IH&r7W+a z!bwZ9{`oWKAmYJ`KS!Hbr3uW#ENg3PWNFzP&p0>-z)%Kc9;9ic6`V7}uy?@a8wkXz zdSd&R^{x9FG{V7!0ZL!#xb^v&)MHh{M`bP52?RVZ^I1oOIJY64o*lwq%8$$O;#kU~ z3Kb^l-rF@bHC=m+KH{*TNXrI9O5XIbz^LG#P6Dc1HPh^n6mfBp^Fr?ph}M5VnZh|; zbsIoIqw1X+#d8+E1n@k>I-w}r#l^&~p zhzfT}K|y3{$Vxxem3m>lG6SI{J@(cemG4?wv;r9d{Sqi!1|f`Sq>(Gir*H$sF14p z?soo*G{rCHh-_tm9PZ>3oK)Qi^ zT&<6IHfV)x4)ZDChl)f9<7*U^yK-+ZOk% zpnA6jBOPZr&+Op~X+Od08qS*?e%*$d4p^d7u0VE@fHtMRG;i^?rLJS?Ad}36vdxK= zhy}t+k28-9XHL;xGkm7eljvr4I|9cR$HI&8efh?ayQ91~YB|lH;3{jX<)EkddeEA$ z*61P=<0UiNdHs2b9i5MYhhy@H5Rv&KSKkXhL2Oz=VdNCbo^5fR88P|D@^9rfv1o@D zIik!Zvu>R^PEF13^iTPr$VyazK@#3{z-}*ZRpqT5-tZs`>yy@I99olfsn;`;(RW-$ zgDNycrYoH-qo2;cQZi}L%T($|3ohM@8yk2t`25Zr(QJLRZ9o;W6#m{++ z{*T=JbXkX<*5R~ljByjZyGbR~ z!&D zFN&`6RRAQn()qL+q)oMMrA691GpyuG*RiTmHS>J2|HT*XimFRKZt6sNUPq2bPq^^w zC)~2E>=wA~IOeOu{qXVw)G%Yj+`H}&t@otS3JC!`1&rrN?(s-hJRaD&V3DEEsVo+6 ziOvL6+m};{ah~Tq?%M|%{ zLqL!#KGFtvAEtfqML_F;`vN9Kfq^>EW9ZOi*C2c?veH|lta5A(V;oIVxt+~7*Z$R3 zV$jc13{x7K_PeK1e^oQ1S5zmgnd zJTqjPAGx2XyVlr7-lALLkvwOQw9aVUy|?!1ctFPP4=$$cmAlfRjd9^K$9P3i3^Df0 zQq7YN^D}g4Y39j0k@Anl$j~&2Vx~$o_kOpyB&?-|UVoS5(Kg=f>f-gcG~ZXyQsnPx zHnH*C{j`qlslUW`>;lTDUK8f%FMn?RM^U_xI+U-moSkY^b1C}10fv-ppV(x|q1P~J zzw@Fkd5C#FgR8D4z5}%u9cajRm)UmbK%wn(0-mjxZ;eTWTmM{eYT7l=ugO9sdwl>UFVXQM zo-+=p1%u`y+tk9Ty}HkuQ+UqR(Ar|;Y6A$EK8hN`KZ>(Zhc2e*^`g`3UYRByaITQ6=n{8V3D?+JgA ztFUEAWv+69s9x$7J;tjGoy=#>QsN$VdGb;Yu5Q~zGS;C}@n|(KbJMkC75)Fd?KnrW z;Vkofjf)0#e+fLNEc*TCtnQwdR$Q}Rc({SYi2;q|*SG#%!C$eRR}XQbekv~?N`cgy NL{G~|vkC7I`9BBVWAy+4 literal 0 HcmV?d00001 diff --git a/rtabmap_demos/data/books/5.png b/rtabmap_demos/data/books/5.png new file mode 100644 index 0000000000000000000000000000000000000000..ffcc29c774a8fc457ea59ec079f3944125aca148 GIT binary patch literal 19764 zcmXtA1yogAx2B{U6r{VQM7l+!`_e5SAl)UM(k&n@AR;XdBHf~dfJlRsboZP7W4t|7 zjKev5ubA_zWt5uAa~w=EOauf39CF(mhpIeT=lHIITxggiek z!=g~S~6ea`yj+9l&=w4~ub_RcP}&-Q80 z*p?S_DEcn|JM#70H--!oDw zSlrr2qVOiFky%|Swke=1sEvfzmyB{wY7=mj;^0wbuCqGH@6w%(GD)X&X5~{7uCNOr*{0$pOyVb4M#;3g}2JLCCzzX zB2AijHrZHTS?cFh&uy>-Yvi5B|}FRtD>sPUi53ngPSx)hAv;N-}#+!<<#~`t0(L7inmN~z5S7^AiPgk zz8VWYx^5cd*zwg(hbeyI!Q^~H4fVB+at%H7J_zm+#Ey7M_217r1dRDNXO{h=HP_X&##rNfXH9uy z-vXog$YO2lG8KwyWAzdfu2k#r;MUWroyJ^n&vVjl5GKS6+2*>*N^BXFZBE%oy`A>jgtQOc(2pVM)%| zEr&ParE!1ZqKYrkV0E}BgnMj8imILTm?z-VZ>!oFYrZ!bSx^#NWx%5p?U6P3o6Fs3EdEZpPAz>a*C44Ev9YWLWya2Nn{u z9|)vM<8}?R3Z+U+$s<;|Y=@@Y_u^Y)(EJKNjYNgY~~E#wgpNLML2 zJ3DK-(l757VM!labJ0urQNIJ9r&4lt<6k@T*G-t?gDH2v@cz}!iO*|GOUr^n>ErixkHI{osWnyX*+iD=m5NIjz#+@1`lJ>wz8 zUOw;=q>De+jFd$!T$U)S;P?_>kyR0?IznwZ(R&Q>=f-tnM_TLraa`4p< zpJ*Tn8wDv*4n+tCjE{~K+a=;%NYtX{fJM?>f!ptP68?`X`}Ip0y|O=R#;P$-8A!2v z&3R_oUlb85%EKQ}3Fv-4r?9S_Y4zC2)LCG*bhp_- zJeL|9b;qM+#tt^GOOs*QWZ(gM%Dwk)U+JZ*d>9 z@g=P`W3;q?6^=hT+o?*IgFj38XVBOWpCXOT6Q63`;hImfg-e1eH2C-!Z+QK^F$xTf z(^KUbtgNi;5=pq;+>4Zi)mAc&$b$Z>lP)sn%YG+TW&5oU4ZfhDV57_0;ICi5CMJl* zjcgi+PG)!N7xwy)+=R2$Sorw)>swn}tE&$dJCNHKB;XPfvYGV|&K7rG?(XR5`1Vbu zWV}?n4EF7$t=T!ke@910=kIoQcHpmfJgV^oXP^)}G(652?(CFqUog?rdx(viuZAuX zsdJH3LxeH75~+!Uf^^{ZJI$opw(5x=;+fPjE``*oc&_T#!QtUya>v$0B_`NI?5Tsy zxqfsWk~Rp#lnjJXJw4Cb7w#52LMA8w>*8KMIB#-YA6#?cBNOx7{cUnHX;L)4IWjWh z(?%5~<1pWf5edua@YlU)B~?|ERVGIv6~>p{K5iS z-%^dYW4@ZTgTwCi#nGl)^PGc(*M6QxDRxAcE=PTB?YM0n6B84sVPjs3&-wn+&!5_S zWR%#bFo%nalB_+030D-@sP*>Dgc$qwCF9lhEmPZwGvCkr#3i0QabM~Tdo@+D6xr(U z(T(my`+2IEGPRK8XM(Xeo79AXi^5mjr|V<~?_0&5mXsJyW#{HnlanXAGBGo2mTFb& z!VX{mmhF4cLlVRhE4y-evR&DB`l~0J#I?y#JlDj~@b~nz98+?U2F7s8=sJC(T*GX+ zcG-y4<7}0d3STEDd~}p~zoQWux{k}~*;(Ji)qz-Y0fizBL5d3PGBh-_m*3ub@6=80 z_roBovG`pd^r_~Eb|iJ1SKHQ+p`&o$c}Y(tOW}J*bqLu~TQWweu9w58j2?v)yl#D| zZWNW4qLIh;yMJP0LZJO(q_ngYR*-Qe6i?Tt-{cIne*P>IX-?WZJq^7;%zbO3?$wlshX-L)=)xXXssc0#bMxge665LVY3qhrMMXu4 ziviW*;^GvIL^%s?((7@FoAJv_VeV)V3D-Zvcmd~KO-)TcZ51gWCv8JIOkG`T=Nynl zrKAthpHEe84^YK@cyFEiRvLxwyw~qrpPb@l;I}S8aQ*_m+F2;es6(C1Wl~ zAzS!kZEX#T?8u067dL#z;e}uAj2(X#LZHM=f~49oW&TG?z52*I-KkxlEn35Zg*_r^ z!~^bA_y9E)z-=6l9%T!=W0Fr<^UeET?JQjX5%{-C$|}DoP-vG;ik~lrnA}}XrCF}I zG|4e6H}CH5axVT`OMC5iw_{^T_Oqp0m;sd0Jr2Jc4;=4K9ihZr;`#3*o3C5&W~h{` z9Gp7|qC8t@c7OHil_KjC9-bi02TzaPG1R>aIH0(YVzZ?VTGv^xCcGz?z(WmjXediT zl8cZA(B`)LTWA82nD@z(N!z;fvK2vi2N*&%me$+LZLZdRX&QWMJ~Akm(8nwsGno=Y zM%TOE&=#tcXqHV%rjJ-1?asB3F4)%j?EI?B$atui8rVHvqKX#MvAMY^$3TdVf`CUK zbu3{NS>q*={q5(^pQhYiWRqgAFxXY)Hkvsrgves8cr##OLL(sUU0r1ug~rs^*T>Av zJoWSE{vy=i`kn0T?2wQUC`@$PLp89~p<(f7^~lJ*3;mLuy0H~F^to0KQ*qSJX z>4hhNjhvc#zt1^oU=^mOxvFY^Op)t;bPvFykY25z6~ifjL|pCHhp?Mq)n6Q~ms-r! z;rCmw0Jq#cA?IR73ANFfCUT<3vnkif?q7g*YUiUr zBQWQ&vQtx2m4;18atv>J%-f6upeCQ5p6>2Cun^NpLw~=y*$Z&g(`HN6H!_-=p02VN z$xt$G@py!f4xgTynmRXUG+VEuqa$&1JRX5ZeR_2x0<8z2I#iqzi5pgd)1?UN`Bv}8 z52DK}E7KK!@9b*LB_sC&iQy2c1OQFx{Jaz->?zy2*#BLV zlap{agf2pHD`!_16EJWgXm}OIZI|%s*XK(SQx!%AdU~u6qG7uPN#@Pi-HzT}kDi`- zun>RLVr##<*^_Sodb6{;3jhk%+e&XNyH16u-+9&4Hb75!a9Eq)YZmq-1O-1#ZNpBa z_CH^mpEnlHuDAEKv<&;oLxD?_PuSd85v>Cq?R{Qo!>tBIW<*Hz(JQg3SA&oW50`7|L;mpP7XJNTTlF}YhX||X^XNV;5g6mAe!25KWKD) z<=O7vzi*41zI?f*zB{1SAO+}TPJy9wPIMNs-4#NQ;=fXU7v<;lI@}DIpD>sGZ@knX9XNk`z;K z|Ge;rHQz6&d6Tw+6mhbJq2gzFU%XkL)-)3|lpX!|R$|c!+E%L|`?iT> zU@6vmcak6bB61dKe4q08@A-LtW$4)E_ZzQD%(!kwaIzU|SwTu_FCmgeq1<7qO^b4@;hg2{A{Eq=i zFj5dBfk3zzgHq=Se`KM36 z+kAl$zp)K*7tU6xoQjsABO)Toos!4KJCPZCS+NaF;2nt*Nza3;NR$Pi?eb)1RR>4M zqvpnTg9e9=K=d$am=Ity_~?d!dY?;N{%{h6Em5ikOl=+@bF!-V5<9S*4*B+lfX$-x z2M->gJ3?U(IIYbE`22(nhxq-%Ucm8>=KzhU0abjtn%iSame(p>*lWKzoozDQ9V=2? zIPVo$J`lwEFz4WCXZIH-?9->#!$J1DeY`t_0?^trhkJW_*(+NQz-q!_ogJt4ycz zcFhEST=$p&q4ZpM?P+TZl@t%KNqYOWWs@NlHMNPZE;sfI=w+{74ROZ+nmagf-eYGl z-}7i$a`J+<_2NY#EI!_hsHmvzsmgy3k!E?b$}FL$s~agpSE8Eh596RT-s-#(z9I;t zJz#MNfIeF#Lh$vgSKBvd3!X-20KK4$XJ=$UcZ1~~(y<5tBiHY60EX1q|HAV7^x7-F&+ic%x62iQ*8Km^g#(`#~G`8Pyk z4h?|&DoxsNp|Af}HVLyZDt={WZf@@4;$mkPb4-`Z*cOwS5Z|GW;E**^Y(@&O-4`Z^ z`sPUD?r3;eNjRHnSp{f?TjsC)4PW4)Z{Na$^_U-wt4iEV1>8(6Ii3Ie0#86mO`V&a zJz`b6<8igquy8xK=ikuWtl+-CI*<%tIY`nJ2GZ5_afVXo(NQ=#VX|q*$H^_gM=V$N zs|i)%qh~&1@|K!RsC0|EAV}t`eVDZMlPcHN%p|&}XaPXn+`Qn2H3bC)Ojlc5n?li; zZQX|eja^BK6QAriI4)7Lg)1v7@NI^iM8r)MLnY?bZ_P+cwLZ?+X^^TZz@qmPSN-4$ zth`if>!jHxGf{4}JBkQkZ<-|taAJPv+{BocwKFbFqGDoVJUq~P`g(fO)&mq;hNZ)IYFJ`vrtXm$sBq6{6aO0tY2=wEgE0uH~u4p*aWGJ(Nz z$Cy`>M)#<^6C}V4>+S0^D1TrIYzRGikUK_~L$)4gT9+*qKzZ5ZG9@!%6hKA&^6+*h zr!-r_4vkW+OLL4k$!9T09&X&E-R3+r8fBA63-Fuo4~QQQ)qKkl%NnuTTWG%na0u)O z7B3OA%CS$|!NGwfjT4|7EjCNN2lt0yVJk7Q5+!8?;i#8utLBQ|cm|w#*4vvjyG4CO zh)n94-E@N~;mzpQSyt+Ba5D7zYhK+5f~~~AQda~w8dW#74V4WbzZ}EiK_B%CqRf$E ztr;0&g<2(}DaN|%dxYI6f>fM>wjfVZN%(^*0}6;uCftMX@dkktOtpiZ-Ng2Z64PT+ zY=mK$U>K%p03txr^!1?@6Q&^wMqSL#%yfLWb8x7rtTae__MOSDmBhr4;6*M-FSxw| z{QN3#@8yoKx5*dIQ8Q~`^)xpFm-Irb*hY$SJ_BY4dYrfS#Y>5+nbT8#sdeBZpkqMi zZJ5n|`xZT-3mPAAhYY1+4OY9jm8D8{Wj`-6l7OY4w#R%&8UO9iF5L2y#Xg9BHd_x+ z5O~=Kf!9D?+l=>MGtW7k&A0j9UF63hNkoSD3Jc3B9zpUskwW(qMW6fIvfRt>l2R>g9UTl&!>O>?=WjMZ?1t@9ZGUC3aQSm$ zLaR=n(YSXK&^chQgG< z2#=O*g(5IKScpOE1K1N<{k(MaMD24o_4Hs@H1mCHTWyYh?O?T{hs>gn#Z!`qa)Zkh z7GyXc99`W2$1|>cSvzA}_b~+|ov$QC#6@z4=cKTiR*xuP_{C{M z`|I25i@CWuU`MbSl&JjLr z44i|CNn4l&^q2icjL8ER7vAr_tl1Ds;i|F|&HpDBT#;Jh!IAbhh7-f$mKZY(K36u@ z5lJAy7DdB!3cyq1laCSdf0jE;)mXzEl1>+gL9#r%8qiZWeVdiF?a}g&7T;Y3+{HJG z&{EErJbhfo-9S0Tx-8Z0ST*j?l>)$93>AjdM!YiB=HxhgsSyyb3q(j%l zuSl(fxy1ztqY_3y$vE*5Ba0bdpWTrGzWD4rdYtbcrg&itUm3r+U4GXm6I-oUVl6at6&fG2G3(!izBqmrBCqeWas{Yfv45 zVt*IU<;oiAynOriZD5*WXDXo+A6c9GcG~E=i1V@xXZi~l z_$J8BNpBfCB(+A97hT;FK8JQ=b089u%e}d$QY#5%L5}qVMVoO9EC44VC@A?}i7!z- znku5FdbRpi4wRTe)~IYT1x=K(D^e5~2rvA^lLzvAQnsF{7G3q6Oq_Unczh55-=WH_ zR12Q}Zc{bjmo*=ixF34%E#ORGW4fq3ekPwnp5qm@aCqTeSJl=YK@l$`E7w+J{jXdb zD-22Q^@|tCH(?_4duPDkp{)SnwFc$xy*_9kP>S-ml5+}BrMEZ;qgGvn+86K~`qYpR z)XciLY3l3s9}!xv;#>%RCi?whyE*Tf11T2Jdl3l>F{U5Z z=6@?rdgr8+ZhTy$$DjS#lz*>QqPw}eCgY+fiZFFe>;xI29S)ju2NF<4_k^lX+ws4u z%LECMfUB8)5~GXTrRn$Kp$lCiC>fwUBKxltLX(~Nj}R0z1f(87|BfjoAkE9v)!VD8 zsHi9@eb#>@-dT_Vkf(2jn4Ucqv);wePn0ay!2wiY?^^A$4m2#(5V{9bnn>yjGa8Yd zml{W~y(Vo_^`F=2+t=v>FnJ$6X3-qv5a9fzR{f2jF-qWJ^6&>OHW%JMsbo~nWrvI2 zMWEK}8yGys4k3ML{SY$@^v2Se&#n0yrN;xzxoSl~7Z_-qNNUJ*lPZdDF#>yDKvx8I zyQp!#1!MudNt5ek62K;(9Zi)O8V zXsZrcS~=woGlsC8pnwHa3$a*lJ8prgpm0#X3*0G$Ysh)WYk8H%EEaLv>< z<(ug0g%XwQ%y;ilyc;X7La471D1(kSHoi#bpL)gTGd+&BRLk*@UVQ8K{S_znoZXvh zdV(lLsUM)Cv@b+73=CB30>c1R5#W?V19+80QKZio@&7*yfD(<6zGFSsRF+uP_AbDJK)x@lM+|85``5OdfK8?stknTfBX&GeCj4` zU+>C)u=+6|kHUnv`NYF`lQc)x^iv3j?A{o^TSVCsD}$HB2hmairI_~<=xutQax=$n z_)kOVc7%`JjJ!jHibRxT9dywjna0R5(21=+ox4!5)fe;zD~^5mS)R@GA@?UPVsgt> z`SV?Z{3`vnC<@Jn_2{OPEt)l*6(`F{U#Y3_w5^RKrqn=<($HqcWO;{CXA1o^YXaJX zr#oSIfJB05iX;3On+I|iF*>A}=D<1%yO!6$YKgL;E6o{NTYV%PcXYwl>)5>lojdMG zyV*6Xf$#h4p{K6}BW(75b80E={uj(PTmRfGVKjL3StO}cFp=C1KzS{;rV8@-K1O-7 z&5D19zKy<&GieFWySKkbYQCxEqP`(ps^l(lxNqxw_vq9j35%#8hm3HAfu8k zMus=9-Nzp4GLHlYShFUyh$*Jn)%)fk^X*fl=nZ}A_~LLn-MaonJWqxE()KTIqcJV{IQX`jk1tM7$ zXV((9mir|=kYwTwt6F(-{t1HW_P4Z=DL<@JWG~!=V>AA^lZ8wDqUfXB4vF{$c;@$S zltf8^;$ca5>|&17ia7_8=x3LqDGrvvdzw2qjfWs<_kM~vDfR}Y zIbVpz$A!JAZTB?V|A>u|GYC50!);Ox@0}H%RO`k#sZs5)em)leXe#Xfu1{2(x@|+% zFPb=|#|+W@o}U~lwv}y@VYnk%tgOwup3ol`&F$@XfFf6EF5W*hO z521Q?sm*#nzVE_=*OZv5pZj6BRBTK*ZXV}fZ)5U-7hT+-O2vk7?p<8Y| zj9&V$;RRk)n4WbNU(^_fSasj=z8y^i;*;-`&pjERhSB+HWxD*btz%2YY~27ZNP+wE z<;xE2Flj^@9Ie3Ql9Vlvmf6|aRE2x#k*uFNuqf883I8|=zPEV8nqaNNmdK%7I`5VH zj5zMjKMrlu>~anL@`Nhf!B~gv4FE2Ll(Gop{Zv1JZz!Ar+@uz%1KSu8U7=$PrU)8y z4(abT0S+?|hIaM4+1Y(s*mDG8>gX8JWoE}xiF4P|&`^>>w}`dTHDNS!Jyr7j4eK1r z-)2q>eQy-SGtP{ht}lc3))2F0xHEep!3Cb9}DPc}kV5g88)7Q%s9i z_~g^Ui?-wU{WF|l$mzM<9VMOruepPa^aWfH!DWvYF>!J6$B&=pDHH>IbFjDP5eQU^ zA%PG3K;-E>>!>CU~47OKxIy zf`^hC8r`X%eZ!<_$;^J3%a2XhgG>N4M>nh9UVtq2Uo94LMP)cYopo$K0-L?<<9WE^ZJTK(4zx9O44S@13Z35={(oGDufvbDmHYMgwkLmlI&;w6_Ob zo$i1v6ivdFv=W3Zm(jkkm+Z&%vFjS+)%4_mfcy}7&KC!i^)U8&9i_D53g5MJKgmM3 z{0A#^3_3dTby6(9n^y$EJO;7f4EJj^*B5KOpJ3%wPIcu@*ghGm`LL&?q@;$xqwH#H zyLB>UqFOQzOw}zXCN@^;&7<@ophu7TU41)Kl$|;Lz6kTt7{M%J8vb+dfOL6-qB8dChrFcJUDaKE>5YW0TqD3j`(MKjx zsB%2W%E=LT-y;4Bv4z&_!y#}Iifxdv!SsCEDR_-N)lEWz8RpWYl#r`Zg7}JqWxx|P zq$4aWEF&YMx|-)N{RjM^w*wX_FU_fHLio`=V>O7aUcP#TC62>^v2WUBSiJbed|{Pz zL|yUn>yH(K9C~s8Vp3V>Ut>fXO^+*{9!;Ba=T!wB(D}2a4?$B)ACliy9aiHe9k$>F zb>|SB$z(Ifx6gS$>B8^Ucj3yIH{)GaR;_)Db{Y6EPZu;4c#e@KcBYPI-}0MoG#O|S zy{h{K^d1z->uXW1rkx%8s>?7e604UlAC2aTRN_DXf&HPAJ+H9li0!fUgTwb@ZbbUn zy07-n(b*Uf5ah=B&qo~niFBXmnQtI96-E7=w7u^D4%naCSlO=iBS1k#8l~{uRxuFA z+kwgQY^bk)(^IfG4wB7Aui=){s&c>7`Hg>L0=VY+Dd0GH(Y-}fCU*=VYy?-OQ@bmh`}=?O-MzL>*5?>+CWqc(%#t2>aNL&h&6 zLLMtCwJ6B>>j7`ZFcw!Uw}^ksEr>Lc%4Gj5I;p5} zaK-xm$5q$;yfGVIDtS6lbB^9Mo?kdPNG^{FiwrqOzx}(+|9q9~+?BeNqqqoeFev}| z=MOX(a9^rwKYV{`>}iyKK?`m;@NKtPI*zv?-Q*kTex+sWy@*3bQzut9U=s&6H8w_$ zQxgQ|Uwo+dwy^rx8W0p`oES^0^|WF2sQy_G%^yhgd}sBR9OP>=R@K1vYD;7fX=2gD zJ~ovp!s!R2k(M2x>&4BjOOqccPd;rqats^T`J=^i4nPDn>cQ#o@<0-$cJmnend*RmdRiJj#fg%n22(Ql$54g; zIU1n!35bc^{4{VhYILF{i`8&97c%hCs96yNQVaM2mq7VcS0MA+bRz0sQ0X(ynu}0t zRf_$cHHP>QE~W;u^bkIeqJU6V^NIR0_W;7O0GPUTiE3mn%ZX}fAV_(M`h%=WV(djr z7zJhy7#6lZKEEbnf69cSv=-ZNK6>zcvlR%Rl=D;7@n07uANuf!8%C=v_-r;3Z7(IRq6Dmb-9RBoNj1l6D|$ ze;+FhdL&jmwSEK=a8mgGW`0CC(}nkaMTEO`1GL7>>^~#jUX084gy*N|GF5$$JHKcs z*fIsXCy;y#CH?BF{O;??_7r8GkmoK19zwehbf(=-@Dvj}!S*WBu<`IX{X0`zu?;sR z5(j}O2r+Dqm$2f9_>`fe3Y*uw_DS(A*9L5k?ZkN4rO|%zC+k>A-fX-~z_?_1hq5z- z-Ym*jFqZyhoFU1c6ap7JQU;PkMCOo+mC5Gkx5NlvC0DJGf)ET5E+6A`(J;$>=K73e zz5LfW=&kjy?xvv+Y-)-$_@va-=}|2lxV9pEuk|u_iKAplpWbWe-XE?y7B<%_jM!ob z_GzuU67u-oGM!=B>xEMw*Zf5Ht(cfLnKJVl#t5=Iw=&V*@9h(dKXf)(Af~mp4!eXL zBIF~mIw{A@sl}f$21q^Y_#VSXMF)%mGEP8_md4|opgZ?j6^wx3?*LmvGs=?VuS-?n!$A9U9J@DErp2G z5()V=_F1u5q0#GG%e2N^#F!)BzKw+_Y2$7oK68+z=`rVNj%h*erKeyNXgHg(4mLIc+{Bg6!+`^xIFCq9V?hpycysod_r7Q)T;eq)|;uRi3xL@%}Wi zN&Bk7%Ko(b#f6qP*Iz2F%FSy>wRB9edr4@>gTtF*{DPTaQ02Pd#EC>Y!?dQb@t8NnvJ3^myY)Z zF8$o>b~E6#uM#0rV8?glpPk_AC(ScZl)m`tYWclS60cgot=f=Kh?MA}Z0f&{T|1lw zYNMqxq%N3%7Z?UpZtrGEP`MCDZ zRt|abYPN_cXVwU45XtnW2rP5>;PM*J%Fs!@UIw2+VwS8x>FhJQ#HJ>pK<82|ki0=ShD(77@K7>C;d|r}cO$FP z0(0VY#)#fHM#x0bg)GUlisT3p7{9*O?=Fd3?vOZeDW2C4K4tmEVLw}3{;z62%oERb z^zji+9+1m^ugj72MKrmB zR0)-TFKU)6jN~KZcWs)<4Vv4D&L~Z zp@8(<_WZCwX6!I^^iG?L4|xXBfwIqx?jC8Djcu%KNclAg?->&}H?PmxV;oZ~`yVYS zu{cWW5k(~|B9jLTtQ<6kGAd}!G8%9UOjUY-_G~oOTP&BMe4YMjSz@aI3Cq+N@}%(n zwk>ey&lIlLn>j_10(p)Rzr9SsV13wsLPJGZP;8??`_)`hOob`={jGO`apm)hVQ%bS zC_&hP5-I0k^GYq+^WI%!sw@9jY>+=i9KrE=OOr#^F$WQR}8>C&Z}*xSXmnZC50a z{y>$^g5)CRAVy{-(i!m+4k~g|;p&OC9^K+gZ9GRv1dQv*Z>2QeyF^&%;dL`2(YC18 z4LvvEOt)d;ac52C2-kS?hgIh9lWk-%k_)}T1BwHo=hk-NW}YIAuPxJNdGebO4pm0CF?c9I%F5K(O2o zUae_V#~)I?o^YxL)L2xk9#WjaD42zOt9-Rc+DWBZ5@{(Q4BBOo=d9C@{Wbpkw_*E2 z$#)(I@Pk0K6va0s89(`-!cu0`si2P!(@r(}<`DJ#Uqu@7 z3=TfJReqRsco_G#TKl8Dnn1=o`Otvjzf4g?Z0^qIt9eP*FU0#a$e7qdnG&}Q)1qU_ zAm`ydVB+>8lzp`C^Y_-v27dU){IQY^-SF}#(fQT&dDfn3P($(P@Uzf`%zBa-^aX)uks5RH@I(NIdYL4;gz)SfqVAjoMDH>1bw&Y)I1J3|w z67p3xZb`DdOUHUXMCv-p9g#|MYA_4t?N`2cGRwyEz=v#7m$zilZtO|&M zT@mCb50u<8d#q$Idi;=Lew}0obg--AaaG7qfDQncF9?BEf}$ zq?hIW*VGT*#%F`D{GK5$@dLMygnbYfmZ(RX2p_JeY&Yb`(Og!wdgo0L!^=y{&smh>L@R=9 zG~~(xt{MV{RQB=P7HQ4JFgAs=zd`yBvKWwm2I_{x5qKEme`hOv{%OS(L*J(>i=c}+ z_Dx()QI{4wo#+E%qYDVu*opM@t8?h&t0m0K*{)ysQGFdQ08blbuJ0?sX;`8+4y-EB zba8Wvii$2SE+8WT7bH%vlis2)IYA)y`d@$oYF=+I>aD)pcX83ZtygJzBqRMxR5{@h ziQm?b#>dCsidyZ%c_}6he5*wEu-L)Oq7^^DD0v05JDqdpcs5k=U*m>ZdKDFVS6LU= zxP`#e7mDkmVbHfEzww7Yehlw1FlLbQX9t|lr_`T8> z8z(;4j+#CT>PerLjZ=O~B`!{}cXLHX<0w0Mbd(;XT+Umf8dH&dBSIA)Ld1ICX&tPV zN3?JtU@Rn8D$+z&X;e6lQ#sn7aC|6y{2@t~CO;X0s8AT>q|{kYc3bv91r?;@Fd zuiL5x-#q2>0c{&%k!P@{qGXnjH^(6AjY!11Zh$=IYdX z+e~G3dDLRi)A0wECl^$$nko=Cqm_jtOPiqsJ&Gs-Arvuuc%^|Nbn1Muz8W+DC; z1csn7I&)}APz-QB{8-ub_4NVY0BrYwc;iL~1N zf`qO9b=G^OC?oW9xuXpP%U4Qx)RZ;_1TI#hR3&;g+@vri5DJ69fk4&>oH;V&L~F1B zK`9>M7h7nb*OP;b)67Xxqjte2YOD|u5*i4vY2{bQ>oHHvAy1<~K)`PQ_x}RWWU{@{ zfV{i7_hG0H-vJBj!Hcky$>6n;mU3^+AbMWm)BW%b1=h`i(j-YJO|0$jVb4Zdxk^k*cF03+$ z?1A^%T-*`AiU|S0cL_yBj0U51(XxeZ(qUaoa2iKJrQ&73pnxjc^1l;gkR+RbRjo?_ zS!8|vUvORyjucAge}7fkR$o;md43!gfBFu2+fgqK=MaB^*)4K?e_ocOUtN(+XT4oq z>^Oz#2C6mSLU2#PTZj1Z#kE@eTwYrDZ+#E!+7r~BzbJAG#^u@&mE(OH>{vqf+{y`> z4%;>yUx6(9%sApHiOqL7hqSO%x;C1=^5dl9^VYNst^(^~QtN++f%9b)v6%5UGm|o? zhEg>6c?UIWEP`>5A3y$gqz(e;aP)(>?Y}Psxp7C(8VCqD-AO-Owqn#g(Zb#D0hEcT z4ol71Ik7ZhRm6JLN)*$eU0Fp1l+1xS|9dDeyx8RUIk0S;WpLU$(<+5733HHpj8wa5 zMgrqjME$QDLF6R*cQj&euI1%6jwXIXKAI_MIHEg|F76pM^)oaoKF`1sn@o%Lr}^Ju zqM>Y3ds4i#u^CuDl0vZ>CI1)`&TJ*7Jrwtv^UTSB3Uv_L>9P7z4ncwEed?1R^t3;! zsYZ^mdAgVTTOXcYUi}eh7ZMPNph0+z@N}F{eyqyK4N{FmTx*gsOdPIV+#gQ!KZ3LX z^&j$Oa0CjDhU$(=(p1&e_1rofhhlB|$dr6h^P`u~E+G&Wv6gzc@6E57a3V-|@>NzXO zGVbtR68L5o5YRrg9VG4aJv4B}PCJ}Y{@vpV%}!q*cBg@5Y_X8NZX8_H5Li1WCnq5x z$XnHt4^t&^~?)X@tP^de2$FdOZvJ3G-VKS&X+S3>$c zjN}n}!@?dU71!bLM{!QY(^n1ouYOkKL?T~BZ{XAw7~!SW^-nlcoZ(mC&`B%i$3$lbUjtud%?0INU54zZ#}Laz}3uDxi&otqVF+9^nXaxRMJh<_w(}PB{TZPi@@0vbdJlJk|y`4tY2yFwV8p(3| zSVpz)x=wQcSlNl$ce6|B#qXMJfY`C?;_hCjQOd!|`37x&KIolNC|7xf+Y7?3{O8ux zxt7j)KJ~2}W=%L_LXd&UPv*KJ&!i>Z5$Ji!~&J`Q5T=D=zHgn&v z4g$**_UVaN`@6wt&pCMrnO>M!=Ud>}2bK}j$-X1=oY{1vvCs4gx!*+;Y8pbJJj08p z(_(0}nVuf%r<qZBB(MA3Oj~|EoH({|l@JQ~C8}uLgLtN*Lu{uU9M?xf|Wvz*=6fKs8tn~hK6aXB@Vb~6@frci!N@}%QtyV)zgemK-?Exg)?RKqJ zJ2*I)o}NDHFrRZgwSy#z>G2TbrF4G9Jlv2ge=-c?(@#IWbm`LA*w~F5H!79N`1m+X z3~>&IPPc%WY59*G;~1)@$UP2wc!7s^jarsvi{46Nd8jvB8|V%CL?1;cxm@n@<;(D>+0b3quh;<| zk4$PNlOa6dMh#s#6NM7@%@yeO6O1MY4=lD13!pRzAL^NwOt>8z8{62}pvi(Xt*~P) zy~XHIZ#{&p-Dl*xysG2;|YjiTjbyVAPaS^Yilzgdn>E1QYfq;v-me z9EZIQS(vWI1So>5@tXI=njRMH$lCc>XdkP9Um!t|o#bYY0wS@4 zu1*sedJX`FM15XC2AjE)7du4m^wuP0HS6oBDr_yg16bIDy> zdb(ylpWocvWDdHkljj)Ca5xMXlH&4L5Z~t##DpCU>D1vc(9)y&Y`qVSPw%h*jfye0 zm{=xNvI`wV7DJE5QW_h{V~^aHbl7-x(EI+BQiH*O)*Qny1ki9Pnc^(Lg3_p{-|zeM zZfm(*Zf0g?e0&^muAV)2dc^#B&w08JpsD+@%_A&edFQ6yn^-c&-c`w;Og+3T7!1zO z&xb;xN~MC1h_948;DX1R9(h~G+CjmCght?5b^-tuVMYdn0j*5bi&0pLHn@&VCPRmm zaa@Ay%N&m5*zRHSzR+_-j_%FG&BB9uhjBYn{$Z`9S}RB1P6LkH6|aWbt|YiDOic4MUXn?VGOgSGX}MTRGiI935i zgzM^t<);#qsftAP;m z$kax!RvJ3vVw&dQ@NA=2tF^JQfpc&?Jw|XcFI~C>r-_APSr!>E$!IycDCYexvAlwW ztiv;v4Z}#KQntKI(=;Jt)a!Ma+u%A3&uh%*=JJ=W6oB*a9& zZ2R+_97XSB&@qq&(PTUvKDKp}5QE*qsrj7Uw&1zC4&$K}If! z^sU1betG>PzX6aBJj3wiLF6SFo}v(r@#)MX`kb51=DmCOe)qfIv0XaD5np=gCEg)k z81?k}7WocY@*@aI>DEmoCm&Db1!*#Z}wa6Uh>lN{6kMK{)plTmXVQ&|z1!{9>vdZ)0)apvoldjU+s4KQ?_@X}GVxU80syoR?>%A|Mj#NN zSRuyCt1KL*-cP}62fVhQ>x1l+#s-oclC{b8E!}w_UP%0-Jho47FIu?XTz11WYJNGQJ|op(B({_S_!sz|q13LKw1_=}tC6v6BxVrnVlONx{(`dThw$<4a5U3;-7Z$pf>e^Lo zUvy?cWx6(_Ij4As=!!Q_g9B;cOm%Ak*MX|_#t`WeN(xYXhdG&nQw)8 z)Ig;giU;%eA|9#wDUOCi~O0C=2JE z%gAb`I@!7;RGFC*NZ|4wu+Lz=2QAqCi#-z#l9vJ@1*0m`Lgz#%8xvv8x0JWY#Y%fb>x;87KSZi0IL_>5R0;N?4mkXt z`Bpkd96O+M#E#Z=*C=_4BXV}}KAOXrFh(YS5#oCEx2)r3xnp&0ZQP9W(0S?ZrORmY z&{?PKuNh}}FG_$BXL{7YHYHMJuDd9jq;h0-Z#JcG;!sdRsBb>Q&oK)wl?0`kLoOtk z2l~UdSvyG#{9RZ~_e@7a+INMbw=I!x3B{2V2ptIGlMtS+8HEx()}p$Qqz&DixWA98 z_M`(G{owXpMsbSt>mK`=9gn3QOA8Bf^oh!9;#g5K`BPTCWCQ&}Z82qp!aEI(J7s}E z)&_$nALcLy2<9^}O zP3IU%!*vgv-M0#w{xJ&nemVZcGd6{qDkXI8a2y@l-;Rae#qgeu<*1eoQ&vLW_vhEw zwX1f#8Dode0;CvCLkp6a%&}~4Evv3A`mCuR(-k?>lSmq7A(ve~6uvSzH-fy6A=!xp zt*1UX;X%(T?ceu4^M3zUGTox2x*OXsf-60hDd?s*e{$m)*p;(tCko!d@O9TPOXP<@ zrjl9-o?9mg8iF{Mn5Kkb^uYEageOx;cw#$qNY}UPhljY2uS1oV-aNA5@k?K}mRZHo zTI)k(6)%WVN zE0%tHt^$V}E=3(L=SS=9?v26K@<88sZ}@s6NTk66;Wg}KUUOmMaxFQdbFCuT%V-?W zvuKZ*!>{Oxq{J98<7_|cSqxt1&KUDv-%s?MO{k>CB3(ml=+VRNT0LLj*uJHQX~!iyYB7sIyCTLtq0In$ zDAmB@P%!oBl+XUwglS55_I>LUfwR_kE!DHOL7o2wzQ68ZYKeBv-*?%Rs0LRgE7GT? zy=d{_(jc%sKCMim`?lHX8rrw`+P7Xh7Me{h8NQF<8j^ctS>pdbm96Xi2|zQ~pIeY#?Ia`KrWeUB;ai&91S(H`PcyU+fezUci{g`9}Nj$1B^v1rO< z;IO()^eEcpT-8(ic*@G3i;j8I4(qKxpCfa9Cj1=cmrj#ftPo>?%v+)Foauz|Vb@rp zG%J!nXRd@Mt~Q_tka~{V1qQAMsD?hhB^Sn1uE+0bYybUHG&5vYy=wQd6RBs?6kFM5 zj5DQrj{KXw-M9er<>1)F^V7%-#hzz;5l=syc$`9E!qAShK=Q5Ec>70XS<;H~MQ)lD z?|r(pyX*y>X8!gw8x_&MjF?)A!K7egzKk&~oJNnCiu~S4$lWJ-=;wIW1t;y-<|xDm zWELE4aV*6mwSU~!>~+lCwa%-5R(4`v(~zUG%~-f22G@oI7}v7%5!(?S(#9;rvEs;; zJ+3DV;_3{Q6?_yY{uSwaEB5ZI@@-vqvE^K&;|9o0Yn>x9Kf76csc;JLov?b2bxRNK z&D-W{KN|=GOMlQ!eeWj*#fB5I=_~+GWLZO$gMlOKq_0fkRcSTF$LA&Hg=?DEqUO;_}6TN@sZIBJILWSNFN+mObSR*tWNK>y879`?N8){ITK zCd3F0>=I*ym*gammCc{B=bOb}gYvYmsJbaY{1d|wval%~dHE(^-MF^{i|Qqusbt(H zjwy$>7N5=v?wCl({&bHXF-5Jq_m)UoFGNHgM}A&MpBR4jgoIW!!}y5Rc@}xAHx1hy zN&jLYX=30oejjV2f8(pSuq zOboqlVRSI{PVyezKTWAXwNu8P(L-y1TvWlC)=PqP&Gk~J#;2U{_%`@S-DV%Z~qWU zHz>MBhip&AVL}mCMeL-Kdab2rUj=F=bvzVN|F%QN`k8Bsna8|K;*c3n#+XG7CBm*G zV;__>vA2((h{Gtyo0C=?noz~r6MHRQ$nG^ekAIkq{>IwViw}Jw)x!MUi?e_}Grp## zCT=(!e3-r%nw1#Z`rVK9FSIytct;Q2m)V5!_@<7muc^wxP44y%;-#HBQ8G01F0%R9 z%-$l|;3K!6Npc4pFW5EI{e{pa+==VLC={qN+6kM=q^mt7QiWrOJ3G?8{b!pO`#nK0 zbw)DWE^pI5?3Cx+rAkY&P&Hf%#{bcXoiJMFp4KffhKpK&GZzwG ztc#{I^o|D3bhSjXbW}k-Os^+1;vOSKL;peN6HXiwbLg6_DdPE!UZL}&btF>u>XSY! zw{MriImK}V5!xhWby@DA^H1S%nSvE_7CjC_>ax;(k4&Y|IkDP>?x==k+W`kkeD?PX zZUeX8I8LEZ1_}Ilk>u=c6=!IKZ(;bWo@P-9fn7%I?EPk(L@00~u;wu*0sq>E-wV`> zqc^#q`%uS=y)|(lNHY;aNxh%y=EGg_Pd6MMW+P*liIk~L*+TK1?VZe<%}ok$RZ+`boNKn7s*Vlw-8^vPZKn0I~EuSu+w4)mX}wbc4*Kf2mLuLIMc=M zs9yb4OyU)^X-4&3j|Y>~nd+*m6GnES{3$)GCf!>&eiXK&8oeR5349 z(A1Ly=UZ71`{=Ht0wYAYlv16n7UiCm2hWHl4Ca#I!-o%xkQ9RyoHV0D_>GE!^hZ|N z54UI>u(tv0kJu^Bc}5>QtIw^>cGb&Ez4LyuDN@0y@Zc8y(VxoG=n9~;0y%&Bkc%^% zou5luhO%)ZglZq@@xqWj!hJXZYQhPgptX1K*GyPjUc*QLRi6KGg-C$ zjApoJjS<_{S`aTX9^+TNB7IGDwFC=6h;hfdtOEgDqH2^CeabIO*8~swrldk)mS5=; zs6_FIYTbdFSIrm62~zEGwc<#d9;x{Rv|m+A@Ej?I_lVlG{6g4Z+d5wglc+F?#xH(| zCM%e85}5=j*9WLYmdcl>`XsF=C=<(&k7p=CU3s{ZwV|a`k&P8*+~iC1T!uzP|L)p7 z|Fn1$RR((`Nh_4z83y-g%T9QnuI)!0EyFec#}&W{N~C``0=`jsf#q(;%r5HFtPs{u zrz?;mpCZH}y0jwI-)rWb2gbeTx}N$Spo#RB;iMAa!RNFkg=VoT?%HdmTj z>h%2<140uc7Bv9!`|nzD{k!qBFYnsBx5bm5gb5mDwWdvoTTqJ#Fr8xIc!%PKkwlIW zdM{0%Ca=E2|FYNTeo%C|xN+I76=EFLitv*^J!Lnk%;u_PxPDgve?=r?3@2PlBpZ>V zQV_)N1E^=2O8bsYCRJJ>8EI%3tu?t}p)!2N3Or4(<=_bZeaygN6l1g;(nGa_nIvN@ z!y+~4sOBS)z)%=RtfEjQg(r=j6pEd`k#TwAj=f0bMViP>9R2tr!hspHZFW3tQS;zU zp2Zr`sH$LvK$!jEXqCmK|BykjUL+QQ?YCec<$<$+G7N~i5_N@%z>mr7H=LnFDMk4g!F7ec%1YJ%m(+bj*bA;;z=CZapZqS3-Yt*!QGZG zgd4PNHXb<`;h;zFv1cc&)nP#zv*!ys9v)TUC zcD>iXa}lTg2>pp}X)j>%d-F?U0S5w<&Mk*z_!R7=80o_f=VKt1qdtLvr;E$G3XtT< z2K-&S72gxd+Yc0ejdOq^(C1@a8&x%7*reGEJy##B{7jEan=6~IlB?{O`#5xytS)P$ z2KSE=^YdV#Wrc0h!NQO|11eZRnK9U}u(w4LL7{n+1k_agCC(J3=7;^z8;X`pz zPqky%atkO?C>l+A{bLMNUqwZ}7Wjnx3mHK26iXt4_w*F`n>>u-qXHnXa42u`$Swoc zXevVncVh2G8T*I{wHk&mSc#&>=5Oxt6oBM33TBTPe=_ zQNh<_DGnXj=*=zYC{X=l6=D(E2F&u%8IWoBFzP%Xfh&w;*p8%&UpRps6?;Q`FdqtS zBdDvj3<}kmlK;~vjpI9PaL;<*kP!G^e0mq^Hyl!EcQHE3lpx=(cTcUYzQv1OMt0`Y6{l1 z8F!mmw~XOTKmhML@}o%}97y*&L(Jo_Aiw*xim3Up(Ih%0M{ge6RRe-86H^^elHM31 zpvt-WM=fd~JR{8Mk5lpzXRvZsEWaB~^`~j*DTY&8@)=-`nUWQ9pG&1klB=B%l0JXy z+zvXm$jdGfGY_5pgq+y$p==P9l?4M)`RX-on@>zxMum$M!>f(l2=<)NS24kA(|G!d zizkB>{$<$5oQK zpsHm*s~n>Piz9@KSRP&8^75`Dx{$YiR>dr%h6CXN=khd}Blp%XWJ>3$J$q!AeVi;r zrI|qsG6C*Xrcr88L8BCc2oK*kkOJu@(0lGg#Tg0{J8Q1+GxgKp?uSaw{liadDYySr znBjSm_i!99YadE0{&grSA-RWvKy&l0s~vP!OO(A6hqU>gu|E3Tyz9&3dt zX%i3Z5<&vryHMeMz`I@f$hqtrmZdWh>g0xqD%2h14{)`h{TL5EoIWQ!ze7q^D2(Hg zy$Fhlt5ON(un;1@^K3Jymn@*XlqobpbPfS*jv%r`lJ5$fJQSbATR(UFg*SjF&03}@^ zgj0$=^m;Rgd4sD4^Zra~DJ&!3 z_<0MOM3n|2Zw3Y;6sjUO=0JEpohPfJIQa@>j@MXDfR#?Tuew*ViT=szUFj`i$dr8f z;GL^Xxe{&7bvw$o`(Q2*rr1y5K6CdXkvn;ICy(==$`KYp;h-3d>e1RXvP1r8_m^a2jvf z4}t8myu6$?y~BFV$^^}SkBwGh#`lcs8RtcLD%zcS?n4MWmHzVxc3}V7JO16e)E=To z{E!)pk8I8-FW6Fy;dhaJj!muY?FI~ZCgf!}2Hb|vNP9_UM&TmqihEZcM#uKk_~;ql zx0+JqkYHwd#>MYg%ZyBV3+G^IPh1!Khn$gu$7{%V{e_`$wQvt48UMq>h_P{oY%i6% zANSEcVA?}DO9fP!U>tmUo>~z6BReFj4}UaGDF{P=vv~1i)h?Aq^~Hx`5OIrtVA}z3 zCAbgiisj|y(S$Tk)E1YczhDdxE6^vFs56bqNVy_VM#Y)x%9xe8 z9ey?}Vzpy;`R8fE+^nl(lVb)zi0prB-WMdN|C&WvUMle)-v1Zv{vzp<^qWF?)u-c& zx8Mp69ftIT8D~pN%US!=>EFK^0-3*KWD3c5q?pJ(+^>^mO!lIFrXoKGR7Zb}lrTLu zCK_%&&S}6O(&wJA_iSf|CZw2rvZ_(Lx{f)93)j*yK<${$mUC8`JVgmK^PWbN^138=y&YQqjKtJ#;qm3A2w^%-zo~DD+0W z+&1j}&zna_ZoR(0+4>rDpDZFfpyvZaWex>Ws`M){~KoU(=f2>!i1130S6_l;aQ=3H3 zUQaDpt2{x&TPy8GxJ=wMh%&9zQh&0C@{95-G^6-jED;eDtg=^}@r6)Yb3vU<6+Yc` zVj}K_opOX`cW%O`OH)M~B2%zI=M8;dme*eaeSW<$e7m{5bam0;}4%f4G;))vForkaAoJPn~Kh9DEOPj2t-1l5DYA2O8f%P<$b{*_$r)c#> zA0D05OAD#e9{|K+J!5F8$PMi3;B^@abZJbY~g7_|j7k4-dYt`@XMtzHe^1 z%8ekj_V(y;MWF7aRgy+_mA-uGc)lEg@H}mXQcyF(2?A+anV3Y5KPGWxx3hJY382~w zTzYg|X3DR{$2XCy_1H8`7lv-ezi=uf6R-%}zg9#$pc5?OhlyfQ;0NnkKp4g~5RPa2 z`uadt`Z-3wxw0W%&=Du-V4LA7l0D!<6)5%6L*n&(U-j4X`}Z$4t0%5gY>yppPaT4S zf^*h48QsMFa(P_4UaxLK)(X{{(IGxt*=C;74LohzJP^_QY?Wb-ejh zX$kT3d%QmLW5i2;`nR~axIGYCv&f8s2$H1T3y5>-c&mnG0s?}@q3Gynbtd?y2%NV1 zA47vX1d-Cnbm8G8Ro}i#;C+RVJB~Ukix46VFQAW>*U9u_mA&<2SX=XCe901OYHT$6 zIejGr!TJ}1^gfq5fKu!Qh01L7C0mjHOSTAzv+o@*JdMz}IvALk`3yk4cYnUWc%SVJ z(@2h!x_xoK{Jo>ERY%Mqk6~$L<@vZ~0!(}OLWkGgX~VkD)10OudT0+&pG(von^w+z z?phKP6M<0gVNj>@xo9S^tMjV$Zq2Jgr!4E&FH$lxaTgL(g+kTRInI9d(ywVfix8mk zXA662FeRU^cVIH^q*%ujYqL^tydzD-G~MR&(2JR6uie>oBubq! zWl{7urR(esc4Pe3yEZ7p;i}%_Ag`dm4)3)76-ZaiLxxomh){3Ri+Krj*NF>arPt?6 zsGqYgD=QPP^o!va5s{I^TGI7f_>ejhEJ20VqfrXa4Yah*(e8CqW-kAMTVe&&_Mo37Xnt~}dfR{8k&kODT&|2HOG z^a(R`8|A;$NmhA-AGE_HL(U{zdOq9t~~WOuwi+Du$cnne78##1R&g_iSXCj_q27tt54 z>YlFrX_OE7cH~rtn2nCvU~n}hZ5ZJkkP@IIs(kd zHxcsKw@r#yV<)GX^ol#9($F3%E}`r9u1~}3$$IkhuU=Ooey;7N9BM15$B}Cb zpv2Wqj-2U=xyplPoC+mZS5D5inx%7fb#*3H3lJU(5pN`mjvr6Az(8W3MCYt+Z&#ZC z6@hxqfQQmgs7(~H?IthAL`Sy=Ernkb`6K8G!!P!_u7{m8c7X7%=-3Z!AVA1!NO)No zu_&Fa%NiQHM{B@=6p!WHgLZUN1FFSmf1C`f=k6sllk9x*-@kulx(zw&_L?LlByrz@ zihDNMlNF4&=@aFPX2e}QfVT!%&gu^^Y;Dn%-t76f`d6BqEWNx)bcny`FyqI2$``8M zovnxVhpZ+%MvNi!D}B#N^-exc9#5uGt{>j|eO_wi5xCV~z-vWon*nW%EQZ;Nxdpat zV|hzcQ&V$uL}uCi5x@%B6CeANBB@9J(bAJpLidbfMp5Pa10l0Sy^|ND#CS5Z57jf; zqyvn}3huQ4EXl%xA7V#y^FNQZ)fP9vhtnA2 z<)G-}6$(Wj5Ap%@YH2A=GT{+l-WJM%GKOFD^pwUt)QA^SC!5xm84xAJdK|9PCv&c) z5nNkZz&hO8+KOar2fK#tb&Cxk0(YU;RgmdG>a(@8+amKk?N7UK*DIZ~s3B7A3qo=e z$m}wLTA3do$G8pSr7xqCG(SY#Vmzppb%ldlgYKcO*?1CZC7dKN>(}smi{gm;*q92i z%`r5b9#85>Sw+Q@Yi%%&y9!^>0%;@RLeB?J!gxMvIuijDUUW@v4p9NaMXs21inZZi zri3ANIwv&~A|N>xsve%Mwg9PR{qt zFDxjSp-BJq^kkF`*cOB*kkbA7Uy#1n|r2WiLPY+4}z5yNIg(u@>kGJFD`D((=)s-nZ;;*Nd z*VD#^pAt%1+nn`}t1({XjxZdC4_Fw(V|{(nSa4S8E~KGlEGt%!Zg-)6bfXkR0er^m zYh|scJmm8}@p^J75q947vG#FR9`6+3OB31;#RH*z19On_4@V`+tXyq2p5kP98f91+J|8p^gf&$X z#=*wsXw_|d;=PXWf5|_v$Heb z@yh*t87uN=Wp2I;>gL;(=vz%&8*rQM?(ULRDSV$VKuC$cZi@mb-0G&MdK+_D|pL-|}UF1cNxa`QtWqItsIn;=*F`#flH@_zP-$d-t?fhl}yeixB)6 zerx4}0B8>&F9NP#=>SUm{L$vZgldU1=z&1QB=-iT0W5)yjZKaMbv#fkUwk^Iti-Z& zuF$N6LS);|zC$2?Dk?q}En9H`2s3kdy|naG_1{n;z&e*h0Oja2;3+97DHFl=0HX-h z6p-Yju<*sZ*8$LQvS7rM6R#w$q`dGB0iA z=g2PB?*Fxc7$@orK79_k%i0H61*EGUPp>RZ*3|gpZd-5#plQJc(5U;jF94nZQJ+`0 zwz47x-!f&Tr>B=g+1St^t6P|qHxX@5CN`+Yil~WG(gW@TJ{;wRi?{d6p)-3iv;%@3 zYyl_8X8rvLTYRnQV-}V*+AXUwc}T?~<|+%I#-0M%#p3GzKCOx+iFSZ&;PSf+ngo0O z0!n-6rQa*S&HaR!FV0-tNMGZ8#3F#tKK8j#6%i3p`48B%VtmR62ZyFz5OQ+zZbb*U zlx;px{rXHevM2IiQJk8qt6^$Qc`~H+p}SZdsX##iDnAAqtK-`%Fa*iqpqKbMo4_Vy zd}(WIdwKaWaTOCAi|uoEdP)uut^i0$$_vGS4|tMY0!5T`0Dw6WQPI$IaSpK}VXs@X za+E*V3L=By{zsc`{8z!;(}C46ba0kGHJ}9KZBATT^kvF$j?r(TVQic9LL>}zbtRpJ z$z!RWj0veLkdb)E1WnP5W)B$56o~3kZ($qVVq}*V7yaR&U^}X-9l^p?BB_BkCA;t6 z2~dK;$AhS~tcG+T&cf~eS!rsR$M^SC4i6$felCq>7&|8ahy2yFk_|vMfq}fi;kQ z&}k2`Qf-V1%e)-AViO;D9~qpr*U|hg7@?jL%|El(R=f$V6)=})~9kT9apiYwI{{zy;~h%bRFFJNv8 zRsCU%1qSb4OseEMU`(J)0x{4OFIU{`gu9G4+nN2esdPw6aXlMQIynI87U=J1|K*OT>cnB);Q zMN$2sJ?y1}fBvh^Ty{s(gSW?>h_RJbpMvo4lfgp5^vJEl7+yeImQTloLVVSUgN|Za zmoT5}H43pMNWxezSW2Nkr1tgN3JL2F>A69z#5Ck^prCj?E5Jw1|8?>4xbOdv$P@xuZD#Fdv*3kOG$e|miJdE413 z&?w!cj{H>s&#V?s#XswkE8GKpEf#?*KvT=&nS&UeT~(LfIX(f+B@tfTr|Q}16=VzB zX6%&%wL)l+)T=y#72BjYUPoq+gs0~ZL*H@jDg2vKWyAS+g|86{kkYxo$;ru}V9}_e zN|(V1rYRIUHj#Ss&}v+`3~c=X>Z&W&S7(iFXtjZW@puD-X_CvsN_LgBQGY=W6Y91d z2lGIC2t)i{3}Fh^SnB+}en@S9R@t;&L)S%^3+ua=yy2~GL8M8*S4!s`nMZM&$z#!7 z163S)9lI}3L7NN(qa1W z5B;B%l+1yH>v1~%sL2+u5n;yI?`IU~lh($gWlieA&wSMz9vHCfNFw? z0E4C}Zp`8vP8GNEST3LVF9iJQKgZWxd_a(ZLh@GTtzKmc^XgROcn9N}wRmMVu+7nx z%;DJ^hOO!&ZKCO_@a_VE86E^gQ04%6K4PLd(n{3LKCoJyG2~4k z<>Van-!o6xIpL*b{EfPjt#~rdKW~OZ{|&*gfndER%fxU~n3rPY3QJ;Kf)K_g1l^(s z3}XH}nqrtM;nh=^ib#XZ+@ZC+xxz%Dn8}qz112dztwMYwrZ7*KuCwHj3?t3pkn#(` z%QCY*BPXTjp<7)2%6Tk?F;Jr`n1OZqyQ*rB1;$u1q%-L z$kdnvYd&nCJ! zYwf3LR0#J!_FQ$X=48FQf)R$E>(9(pg*g@E;}cV(N`!GY`bg1u`M}{%M1|U%uzbS9 zJRtSUIPavE(l(c+pU_M(vX44AkZa>TG!bB)a?FpegET@g&q4Jph`D5`pY~y1X@4M^ zZel4U^Jg@b#^rW)&6Hz zs3{@~8&rI;1^+I-F$>&oszd0x6D@|s6&pQEF5)^JmK}e&@5H|Hm_};xl7G~h0Ez*8 z+tAQ3RJx#FxHo# zcyhW+8L~AKcsRnGL1&OGP>mWHqp5BG!r)`f6~2PQgs*}Ah#?xC{LNisRJO^aKhH+R zlf6&wr#xIZ_98ZOB|$q)O=ul!(; zwZj!A)mh!NcLyGsyZay|svXQy^k!v;S8j}-(qkkv|Qnd5Q`s^osza7$4!L~<#6izFmOO6T&l;aS*9_vTm6?<%+IGI z_L|JOpVYv)e837eJpuQYvM*F zhOVw`1a;Vx%?G_slTg&zX0?Y6TUNeIt8j>Tpuw%B3^xhOs7$H&J9 z2S{WbNXQ?OBlbO%w{oHCu(W9Dk{@ve$bQ>dU><1$3u$2ikn`d!Z2d2{c@$G;cL2mI zZ-(~voOAM9!9fy#&(?(*SMub14?`b&j(|%*KU4lY)Vm@uuf%(2Rjw~6gxsD;%Ph>_ z#2sUMs0BdcKZ3l^6v*UAy`)Ngx3~4%d$v^MEEU3Z25F-y{E0A(9z8~%J z(_~66D`Tn6d~lT_PLo8vr(My5s`b;kIl`hR7*MyH~PNu6sL2L6ir!iWqhXPG40O(1#r-rSLtQ8mAM^xhocp7Z45PBG{|^YyQh!%r#3=1w8lpdBuUW;Hr45X;z}WY5h#{jo9?^L8$uT{CAwU z^B%{EmbZHlH@ycs=10&iSRoJ3&lV77d5mo@c1DA^KE_gho08x9PK~@N~ z7^Exm*|kntky20qK|v-ze$0Z3_q;_pU#5R(!AQ$7eMNZ^%`HraiUC?E#!^>|kX7z8 zQuu(X0uWH6qtW4>(dq}WoM{UGb!CkeK*tk8(wx=7`tVY`p9XLschb3oE zKsN!h$fUu51}ZITsrg7)ZdIY`HxLpwHmJgZS!BU`3x$97$#ftbt7?8B8_v^+G;PTY ztWwmZ{bw*nA!1({gS~AayeY$oc9-Y)hyrDf*g$pajU=Xf)ItoXE!x-tpqGF3oR4zGDLItZ^ zhT`KjH8nstd1Jzl6ziuy5wA~On^YhDV4)RcX(7w*p3V*#g1_W0?$&~f^`{p(_<
n_{}Y&iPj?%LDN?M?R6OEwOBq%gxM7A! z3LPGE!gc7{0$I_dCZ-hhBeWMi2>&=A8*(sY=IHDkv<_oH@k*!X<3ToHVGi|GMS*JZ z{r;gmecZ^xtgt#(>rc-%B{7i|QB$-_9S)y_f7mG$9X3&*HHR=Vub0$&Nhl)tCnhG0 zAUzHN2)V(1px3Rn{~&pc*yW53gy7sNdi?h(2m9Q8)o%VuD99(K^Ycv8U;H8LF-vcU z7DouJLPnT>dPeR8H}zW3zZ}UDTa?H+SYm^cmL`Iag4nGZrMX7~E~`N8E!NYJxHyU# zBe|?6^E#klM*o*!Wtg}2J0kL>76%%a4e2FT4dwRk&i{bx>a=bkmj=n{ytbd*0cwz7 z$^Ga?AntCoaysPbVM=Gzrs@FoWt$HpWMb6fW3lY12z*Re4%e}_j?Cb=w~oMGSm-TW zpiWr`HVq|0K=Ne$z^CO8rfkCk87;=Q=}wVE$U};%p=yUjsFx%AR~5j)07A3xZB6#c z*_r7TEb4S?UML5`E$N-@ag|yQV(SpVk5*g(lpEn)(+7tSr0gl$NZNJi4%*_5y??Zh zO^v*w3wr2J;Cv%LEo+DryxSOrInXbi^BYI-L-L51GYd>A{x(ga_Vh3JJD^xoqYTMxP6Nhw6(k(uT;_or>zBPmeQ1facx(nnIt{g_UW z`x%3oGDS3c8ec*h&M$`HTrR^{su(XHT~wPj6*$8;yld{Jzzf)fYegP#Utx|$6O z6-pV(HTXSeHLAP}or1{yH+S)@Oj=-E>6GD5Hy{}iB+qU6nyPQAK`CK|TJw>5x{`)h zeFR5luC$VAk5r-^WfO*Iml4vF{_-4&nUT!=+DS^A=z&_uq?mQ*5VItK{`iegwogJ+b zfCuwu(qvn#pfkXZvG7P889KsaESdTC{)Q2rK%>v!yL=6ur*O^0G4HK{Tbm zVzcnCGpPEHJ32#jjctSj0ErLZ;WzNSBvF6n|`ozaV%$JK9z;*g>nxd=ReQXuW%DI?_oarCB=@q z47jI{XVfFInGE+YEiHl9uc2LO@G?i3B~>&(Ep3Ftpk7Kk%-7ZYj^b0HM3c0t`Jfp( zD)IWYn~grObG!Q~&WcdrkZ%(ni&yT+k9e1&>QEf#E(2QK^lcKf;vgd z?+VU6m}>@qJa8bK%Cqou3Nb}Xpq_w0RFw_VVyAGfbe!Bf1!-cnscY>{tWF!rCv1r? zg>H*=EN6=0L4{nh&up(K1Vr9sOQ)+PX?v2q7(EM(va=S|KR->SK(M8;A%_?L8ypBn zQxmQb4pJjLEWYae0Y=?FYZJ#o+B6{~17<4A3iI}Sf98*(gs^_mxZp(^)G_!lPxsWB zyLwJ+D9vLSZhy8$O_8I;$)c^UenlAhT7aTG&)$PK7q;f^pwZcU3_+7S? z1My%nL9)RALuJWQp0LUhXnz4}VsR~vdI*~=atMBel;VF79^PNym%{J`;+t}pw&qkY z#50-V<#zseE{*MhzLtGu()Ek+QDHU8iJ!Trxy}(lv?b~wX-zevzez|?MssKeCf4?) zc$fryp**)wV$?8-)99WNz;8g(RY3MDktN|2`}^-hva%~_&+|y4W&kMA z)y?gHC-)kFc?{elF-`h{Y$NL(Ht+SjjkK9;|DW{Gqpgd3kfcl0+kf7(&xlBy}q}9F^E#dnefi^+?8u^K<_GQ*g64 z7TETCrsR8uiZOw4KbF^2SMJYfsi5!7&GSfU75}Hdes6!+1NPJX?eh2hPofdj1rqjB zAA#!(Cq5#0;4d6ZrqHao-jB|0v}6Yt2F1pWllH%qRc9Tr@)Ua=mV< z@E_lXvxsH=8m&?`Kj%JvxntI|3P0Z~YFAyB6V8V`c8yvp-?K@#0t>Tr4gfbZD@xmV z7Ik*buxc2bh5Lf465*hu7{0Vw2hu$)&==aDSbfRXW<1|b-g$3@y7!_sDVh&8z}=~QSp`P14nm<1 z@o!XyPW&S%t7fR{<5H$O{<@dP^0|;?L~^t&hIB;*sz3Ui>3W8S6&j@n8p0wXKDWn< zSNjtr(IM*h_X4Vezo_BluvbORopSD_XQ4)R9b0`TYjC+F{Js5&UG`%abB=J9{=O5z zg#A|SXFxa@R$4Vy1WGjl9%Of+0oX1lXuULO z{Le`r4MXpKP&lcr`8=Ii*F`))gM*BP;J{N);&L8?L@zj05U}XN&I^_)c!Rz1sDYwCsUNpx6zr` zBl0EcoHDdn+P69W#&p^~29evj;Y=o2VorH*j&|V8m;jjq+ORj_4m_eb{Z>*M5#?AE znxTXWLLUZG1JzlYYWP3u{&1(uE{LxCp0)d za$$WsP@FPVJYDk{%qBg@3HJ>ym$^)i) z4pANs@AeX|^`N)6fC$Z{sb9flSNPP>0+E*r9%xDE0PQ)VZx0jm^Yh87kUS$M|8SAV zm(&Z&x=Nc5fmxYr(Pu^#rY-&Y8!|1o&lPfTqY2H*oGS|DX+=^^JM)Fr)cdO9{CNVM zf;(!I3%w4A{e)892Ok5qo=k*luPF;m(iX9UK@(H7OuiQL9_YU=1TG{}%@09S=q-0( z-GR(wjZE8$Dis_JUrj%xeFcNNUe@PN>Npv_w?FDSV$Y>;v)>|Ho*vZ^&_`mgM4--q4Q z{$JDuL1hq$oP5Q=6rl&KIA z{=lHtQePQGNt(fWlTeK-^4nzW*yIOYeJjgE4F;nz;MXX?W2F=2c(#qVa$kQH$zESwZTs=Wqhu$34a@#powoe5={CHh z5_uH6ZPK+ya&&fb*{?}LzWUi<)h|{91#-OJDt7nK?UKYK@tL=+O@C_Yv%=MrV~P(h z3@zKKRF+}bZ$yq5u2BgSnXpZV2LpL6j`?uKvXNYjtoj0kLWnl+vpm}{Z2M^&YKh9B zT%=T-wX3;#(v0-gp`Z{+|M$B9>k{VDryKZk*|LiY6cWp&Gu3K)v;uX`M3P#}hMFA_~kH z44pvLNkoeHV_g5BV1*K4lcn>#6@n}wn}ycG+li~;j^{H|5*qFZv67GR*6cH}()GL0 zPMJLWRfQ?N60A&G*R?giej<;F;xE5!D@td7*+)Jn5#QN@Qy3S#j}ur==Q zXh5v!6T7e|++T7O1Ty>4v0w36YY}=dK;uk1$UC4tO{3JjW)axwls*{J1fGq49Tx7# zVbXn{bfe$1;i3G`hUWW!0f_{5`#30<%l&>oo6RDoQx>oh(^N5KKs|xm03^InD4@2{ z?RIOm8WN*g91b75NohY^+n0Q$lM{Ayg<~_!oEtD2jW`-s>MJ!L5JXW#AQqlnc8a*R znS&0rv92L@3OMt8K3}O+Zr!?tgS5a_CnqPEZN(pz{7Ywns+pb$Z8Su~;sb0r(8<@x^?e&`yMqrd3+I#rzUAXK2SJ0bI#Eu_T7%JS_SzkuBq= z)9J0Pt^4=yfAGNv2L}hqWU^MP{o)tD7>~zafBp5*(NQLoIXF1@#V>yG=9_OqLniIn zi>~eEp$uRvX0w@xI*GFRL^uq?;qd?d@Be@oGWPIO>2AVfxh%n3Gfwp_T{Xn~(%lMe zx}MyD=WH|@N-3yvfS_@jni3^?pi+^$MGstHN76_0P)9Z2jR-#&MUjh(3m&45Ea6lP zU*YakP^b(kn%Qjj)~#FbzyJORAAE5C{(TIQ+TY*5ef#$I_V$1L$A3I{@Ss>M{_0o1 z`skyNc6WDkxm+N+hiNb(n+V4yjK^b-Eg{MdNy0qr*Is)KSP=F}1ftz9#xmIAMdo6b zi6)0UD|af2qSMpUhYuez3&c?gksXGNggNlID2c;?;X`T>6{-1g`?Bc)7{-& z&-0!?fByOBpFer>q|s=wZp|z3BHBW;aH&$Mw7tFk=9_Q+{O3Qvckf=YSj6WOiNxmS z=9_Q6+3WQ%qVTP^-rCyQ;s7pZ0eul8|8O|GoRCPV1RQj2Z4C;CDg~?ry$=Cr8Y&i( zOSK>=v2-DF1e1c<2?QZ4`My6IjTrsGn?&h?Ng=uQd|+Bp>gbb`lgE!AfBNaC<#IWZ zNOZehR9h(rG3|u^LPJ8sq|<5OMLzi8gV$ewy;7+tr7!^nF6I9IK3cpTd$NXWv*s}c zNMbA7L)7VXF5M^oFjX7BMhBOPTQi@SiUbHK#+4KieloVC5+fn`qf8m_I@GqXAK^S8 zSbl((oRKp;cqpz~t%j@ta;7K@cPo3G}Zj zJCA}Od3u1_0)mt%TU?z7N!j6|38m!XT52I~qArpv<~}Zd%-H4xy7S~{?y1b;1a`o? zyn#qeQzc14Teu|GhZepa2oNQ&1LrlcqqMjfhv6bn7`Z`AmMZ6&b!wI-Z zgyTAhOHc%p*&~L8kSaEAjmP6lD<6A8y70uB1oPwjVsdQ+z?*v{Kf@QaUF3_vp226V z2h3lgM8k=Ymz6Y73vpaDkDV}99ceKxEfRIf*oUz~JIISa!9q3iiSQV)GiV;;c54ee pl7M^}pZoTBV5He`&`rHw|9@_1Y&FdKz$^d&002ovPDHLkV1l85bP)gm literal 0 HcmV?d00001 diff --git a/rtabmap_demos/data/books/7.png b/rtabmap_demos/data/books/7.png new file mode 100644 index 0000000000000000000000000000000000000000..e01fc98f0d770c09a922723177f3bfdae7d21930 GIT binary patch literal 13749 zcmW+-1z1z>7bZq`cRFBnNH<6gk(fw~PLUibDcv9~C|!~x#7TFFNJ$FPE!`>n@9)3o z8GClwb9e9eo%6o$IqzAluC_W6K0Q7f8X6H)Lj?xBIsnfoTnylEhma8nc)|2khU(+u z;x4S}tN?HE5E{mwXlTTJ|2@$&c!?R%&{)x+DoXmkIi+UCo(#XeI~rRj6m7V0!?HD? zPwAoS6AnDz_W7vXGSbs<6mAzcm$!pgGMiu-eVcukl6iDaOmQWu7~q&7^{aNjYhMK> zQAof0Z73E-`=5YAU@}iT1H_4LB&XfbiiCU& zcm=OX#}RLcT~g@uBH+}xduRwMp6wz^AF13;`!stb6^zcL6d+|#!_U>5vie-3ecR8S zQIMKIXfZ%^D{=Wb-1o36Gbhz!9&_V)D%ks3UJkn{R4kQqDCB4Z_(`I>6XL?BbM!daYv zPA4VmR{t+w4Za#4QDNNpQ_JBw!z*|L^d za*mxL7FH}CHZA1;olow1!Fe)14BK@sj3PLv`p@>X;P|4)(Px}TT!eU5|^q_ln zL}ia{D%hBh+KQGsUKM%aE1NfBFJT??c31M=tc+|XA?i6^A*al-u5xP%M3-}%`84*U z7AN5|AsR@=@TSAF?zU#r8ePc7nIEKH(TQpB#7jEzvQ*YuQ;(7=z8{Vhr%$wm6hnA2 zG&lSzKqvdVUh_v+>yo3}XSCLwY0#l3)4`fL!*r>Q`EYymaCr3YvLV0UU6z*c^d_)S7eXCB?TFHaI3UX= zcdhHo^?9PBqrIBn+x+f;cX!+BI z{&WUEnk?VW6ssr6-|iH*x3>d<%$%fDjYJ}IbD74@jtjt0>~M5iKl%JI`=?ovyI%Bz zCxO75Z6LPAv-vZhSF>N^-CHFL*@|RmXE&R=)*FH9f@oD32gxM_spGpX>+0+4o11}{ z3uhG?nA*DlcGHO5lDKrrL9|kzXWG#_AmFZb`JXE(2)K?$`MdoFNL)3FhK2^dXIUge91OkvC%@ByLS5t@1(q!wI3j^P+}yNl zHl2a%aHe%zuJ|zvFcL?vF#M1QNkPdI_z8PiaR=7jtEO98T6n1A@$)bXKTwl0G6M@* zG&Hv9aT6$?Ic)4yvB>IAI;UOMV8q8PA83R$sG6=s`M9|qP3kE8`*#$)ymz5l`fHFS zxUIe(J+`B%>3Y9m?C?)BX0A5^QII}AH#ZIkR;)}X-_^xsdhf!zUNV2oqRn^05%pu! zqe4G!{aPpZcG6j#H*?s;#3YGD;oar_?~@Z@P9W~piYI_vWWL`>(*ZIh-G&qd{J$gM z>aaHW-yU#E`M~SriDGpwF0Ned*G^8nxM5&uLG_p4`yS1VgxJ6_0F<`Rga}bHzn#C$ z?upSSO1{S|l5tS~OF2%=$2929nN~5o2L!V%elNn!O^~oRF)?u!fg2!7O9)3Apx}#( ziw9SC!PmVk;^N{yK0fB{{@(un{s_d%-8=dG<=?;ku8*z$TNB3&&CN6&dEo)HP&YR> zow7;b>VU(h0?{V@?m!;tXloy%;}d(u7L~)14~K6%@7Cf4@gsYHy>}4(G`5`_1%?9W zPY_NU0#!X+pRBtD-z|`S?tIwme8}#+$)*uBH#IfgKRk@Gnmf80Tz3cZJ#*O5&~PkY z0Y^bOT*>Xyzx~8_-_Jb6y-VY+gCzCYu6EhWK)NWaQrH_&`T<(po~0##QIo^c($avm zAKOOFl$Di@*n7TyUAE|Yb@wltgrx`Huxff_w735o;>y3DRQUI@b@4Lj@$z8dCh%cBNnzAh1ilC0Sf*y_ zP2f8(FVS3Wbk=m(M=g6`bwue8j*i@1T@R0r6aue>(uOGUvC6Mu73m}PIyyQ)+^-KN zo!tZi>>`_`U1n7F+Zo$MC|A1}f?tvnmD4M~;)mvS+#9%@uf+-6L4mz|e&Y-PJewXL z0oWH8w{Q6E_U??q$%QiwI56!bkq62XewvgnF} zW6>VV9vvS8&^R(OGU4F0J<4PR4q@wcK zy%(Hm>+9>o!@&1gxVpL7@S|qh&E4el)%8FOx5E|>sXXP5dnolbYV3U*N!wva4F)kL_(t2anNi46K3_0%c*S7{ zr)vw?axk7Q*|Ahx=9t3c{mxFg5ubGzkj+-K05N9q@h#bkuz`JVPB%|ZP5>PAX}!C< zLomvXg&PVnCkYA)=F9q#gSz|sRa3!hV@0ANl2ZclGy}JeeD1;;fjB11@ zz0oDpmFBk8_EEb`R9fgW8otpOklHMd*MpB27LS{L=D>DrpIP>5mST((Tuajuej3>d zlm{*pt;L;Dhm)do>fhZ}F~tA&oO`9}=#KTuRwNX?9Y_)D`fDJO9Z~ZG0|UT1)5P}! zA}DFFcmfw&0ynIxiq}R91t$u#lw=&WfBJK6f1i*x`+cS`XIv3LebNA}x0-4Fy}!?y zAUl3X8$~Fn>~3~fsj_5b-twfbQn6=%)*D;MKYtb$UTV6yxV);G z9v*&vOj)CxWhI_{;V(C?$BiW_+365!i=k5^U-77o}80HSh#eE1h1AAb{QUOU@) zlIXUg1Ymm{d%5gdwIj-ZZw3iRUM@fWTYmlewUKNA{?6|S2hopOiXuBs;{Y6Ocr?S2 z*agEoG1RNv2*MhyRseX=c4+po6$fKDb8W{|MPJrP8bmEwAPwTzecjw@&1(&LGKOqn zJQ*v7Lf^1Pv3d=7sC-q6Cp4Vo^2c~He)!5pDFOuF+uK{)aqB2V3-G5&qw6c43>*^X z6=NA0ndSQ}iS0ALvPrrmAc2CfSEE~7Tl=la)(B%rX1$wDzb&|gm>;XBf{ziFzZGAU zW9^1cG#CgtM^mK3(5L8%7YIj>MRXItKG7ezS-Al+V13<2#h^g2N9fP*-xUT`$GnJW zQvpLjG2$1T7DHf~iGwz@?~vZb6Cnyl75d^560M7e@ic;pUQYbSxiFs0Vf1IFW@fp- zBR_xEQH*s2;2d;z=qI-*rizg>vW{Hg1!6jd)xw1AFBqcb2 zP9;o}ItL<$@nTEEHeMyq7}lDvcJ!_ynoZU8ynKDHk5`}dVZ8gdXMuUr{RV)jar!y~ z%ZC$R!e~6nD)U;KQKQOLgs;Cpy{eu-<}+Oa;wR^v$CF;tE{zMMzqB|pCouCA3EAV= zZ!epucc_@DvfifOmE;*3;iY-l+9{W?Eronbj^#2#rtz&_(F zKDaLI3IoL4!FZsjX@y%LjZ7a6qba%|pimc(nZQHHL?=)7X7p{xOFn8Kl%4lHn%i1h zPGyf~kS3cRenPZ4IXQqDRxZ@7ovp2@fg@)CrBrzIa6PDymzTHp^Id0fU?Ah*$>pUu zhKdj|fRzv97q2bzyKokk|;z>0x}@vIRI8;V`B;r*XuxxM_Bp3dnf-< zYo!rjK3@eRavQ=z3sMje<8`C){TQS;;BXd<2TKh)Q<40!3rAbY{@5RO7zx7N9?hUM z6Ph`KUeca&!yvVY}w17WfZ*8BV>BIdvi0~n~o?+jbpw{tm~y2_vmfSUCaKnN=M9}Pxi@jN(zV-BMm~a4U zHZU}EPiAotAF=i6>HTCdKg1K>!X z*2Vv7s3c~&TR>5_EFR9x%q%UL*BeFr0z~JkgD6ms{+BVIbG31#w3j0a?CPQSUtRKCXW(b?J^*h6JmR+c;4KI@b?XqIjsC`3xzqKSIds;{dIR*nu;Bn{-eG0?4TpA#)KEf&@<_Ldg>dQ-z zkH(P4r)wZdwYekv3zcwtliAP%pLJrK_eg-*E427SWaClaemnmslNS~j#Rl%TgCDlf zw#NWz$dw=H;qHzuT8ldLk_MsykS}d-kC@4m0CM+xxWAd6enlIla66F%lpyPVOW6`` zqhn)0UIGA6HLW~D%-nQzr{WXSZ?mD;0C!TfZef|yE*X|y1y2r9ERhq(p`8d4V)2cf zi#hY6zdyHn+0+Lo;0{TXWKIHV7Aqt_b^L+PT~%#IOH= zH_V{w;y+<^>riimrhub#JgD$6LF?)5jpfFI&H{=c;~ute04VkNnAA^%G^~IID%j`z zof?Ff&d;m)r;7vj=n!hVOit`K00oWDXMssYN`1**1K@d+B+TU(qp$`LqZa7n1{D?pds1Tv>aS|_+R#?J}PoDQXN z&A(yD%Su9Y;m^S2WH!8&hOFNUE@oy7iYMk47Jw8-E7EeBu0)k#RS6itG5@!j?!qH`KvQ@$QNEOQSW@R%Z zAzsNzulS7_9mlDdEZw@zD(UyoNYLuBME=+_ZlKumXwDhiCNl_q?7QKDD$`J7M_ z`ofa~rW}vcP8{X>F6G?TLW>mrWqh9jee(Yz1Bnlic{CRu__9)cPj{+SA0RD*=@z3FPDRaUu9q;+#n_PqOsB#QNBU zY8aQU;L5o++vl{I^;29jX_r$vnm!NTibdNrVftB18Wc4lG@!`CLGs*zv$ITWoS4}& zFg0x8fKFq`R}+@tN#(cQ#}O4O)a9?Zo4(fNQd=b4FZJbexOz*fVi;DRFvZs2PfMXl z+kIe5jgbG0PdR`U%`%{Bp5NTe?Ts|e{pt+NDZ$T~BK>fYa~Jyq{mrgB!Y*dFq@_^2e)KOz@lZEi z%Ap3)d=tA{#hovXr-BV<^*@Z4^da$lRD^-_pSB`7bjMbQf**)OYu4JhfLlf)(sb7eShFn7*?eM^B1F z-8l9>^Koim2Q$3@)mplx^k7OPx*dJUXB! zM$KRk(C(NWimcfpR_L|E`NEr(e!dZwt*@ulZt}^X0;E1jk5D*RL%1X<{ImVJiXOog z!?Epq19EM=PP0$_D%W{Og_*Jr6jStbUcNDA&l3D`I<>uy?034~=BFBUj`(f$5yfsPgzY%)#SWI}v2wjaKGE+ZmH4h@G zbb|MHz0!B=5`?G_EqPyR!teD^8YBE!>FJq|!!?crQ|TzQOZ3h6FtmMp6$~XFUtV1* zBu;r0A?H1Q6dEmh8Cuap@+)Nu!IL^NcNA)w7_wQfbL7YR@bJk4p zC1vfVl(-FB_+FtKU#s;DJ;7p2sA~giC>dy6h_5n|j#WD0S8&l(GUGf4?+J&bGCuJ? zB1gs2DO-`7m2i>-;rRZdEHOm9^$TqgFBBz1O~)i#wG!#k1gL9~| z>A*ALOPMNdqfy5uMiQo5tzW5$pxWA{9W}i!YUygZ7q_LG zyfmExRmVMDz2UJ+L6x_6O}I7|vM1-Oqo4o3xeQb=2R;m68)lT>AyKShS8}{Y1uXyw|_GBq2DC1=e+D;jl!DAR6eEsc-H*E){ovbX8!Z4Yd^I z7ak&BiLpUfo$?v_SW%?6e13itW?pX(Q}PR0nxHoMTr~a4(_CnLkddSJq8M8R{Rj7V)u6f7=b%=m^XGw>@@;$ z1D2w`!BGUL-jY6!6~Zp(LIf6-72 zk39*L0w8zcUPv5i|6iZhIrM03qlVvvSLg%c>~ZU{Z4p!g^mE^g_>`Y2CQrz8oynHZ zZaM~tenBY0Z)<3Y#7a^TzER%Iw&beHT(YVD$*$F8zb%ZPRj3|o^LTsh6w3Ex>LM7i zv4I9NAP&Bbb{l_gwiH2I!aBh24O}6g8nQO2nz`NIca~&Sc6wtN%GAp2+BDwOWTkkA z?}m*|VkCgP_$79Q`J6sl@4GrBNPthD`6B)6C|H`e;UtakZ|a{igR{pU7uf`1XwKxS zWGe*my9Ef{M1rc+rl6*ojOjsF22n3uPfNhe0t~n4XFMFOpSDr#E4Shp)usk`C552U zpLOK@2e72%*ro<8smGr!<9~Z|3DD4}wEmj~h@VOzZJ}H}=JjraIkL47`wQF?P!TgW z8u4X$Xs3d#2@c-#CBg8qRQ9bug7H4-gcc%_LoE??*gyI?hMYvjLdgY$519OT$TBKr zUpBXRKqUoYgt40m$cf(;%dH^`tQ{u48$`gHn)%=%=iH^b=%5pouOD50ThFrKRgtSs z3=BlUDgh5onyHS#q_(f9fh3I5ixjn!Y#+II`C(mlEc`{57N-ydH*%8YeCF~E)N>RK z?_*EPF&U@5YEw!{S(Z)Yo1<&KimAjZMR5TKSw>>@VlsSDC8YtS6jhLd@>$ouc=n5P z^b?hF^fXnwGHN}$H6AH2-@RDj8K?e9e>Pb>LmuxWi7ttd4-o~i zbF<+om=(SPE})5BFKJEgLwziD3b#UcvNd~%q54DSdUsayT!N4j*-pgv<`Fn7yz0`jB-N!csCxA$eX85;%V_AX z_8TjxWGc5im2QFFKEA&nl|t{ORgAv$xruvzsRL!=D<$4WpH`r+oDFoleydbbFJBdY z2b{%H7|`nn8a1&_wsF}n3nv#W1TxF-qnZ0sDTH9CifQnM_m=^9{5Mn#w}|eu1$?`t zeyAx-ll=X4Jm+*vj)GVXHI5WhCE8As{cuw_D?#)T+#WE!CLAJM?K3kolhx~9Aq@iV zuicwXFN0Xje9?F6G-h00bvp=it6<`UqvJPzUx^(y;;6|9ebVeIYLhb@xkkc=ArNBy zfqDd2Fiv{(Ef#ssQ*MVD+2D3KMxUUP!^{I;=5sGQsA)JFHMm%S2;R1QbafZB4qq&e zH=>OKY{o@bz)H2QcR*rsSFo2m${D7jVTK7ewDRJk3ei*S29?b_RYTMgrThX(5_>t; zUIvkU+q$ibj`lzfBo60K;4`caDMt!_Ml+24%s!g|`433TW}| z{N45gT#cr-Hqik_78ZqoOJKSK=%B75V$8LaNG)v|j+7gk*uL2a?<5iJJM`kOS(l^5 zCa*aO+mFA$7@T)`%H@S2&4ybf^|?2BYlll+j=AYV2}CRm_5rk1w0)8{ewQ zu-HiFekt}x!Br!Gwpx!XWxT2u=X)DgJjn8iF8~b_0qy!ch1nTguunTG!&y`_-zzQ_T?m~BQzai2y@4sg2I*-0)Pv6wb~OLC z7HM(MmvDA$xJCe1Ib?eZH1!c6wWI{l<%e#C!Kf3U5$r5pkD@nW3T!kJaG(ATzm}C; zlT>@;C|GNgX>zBij%N1J{p|1>&l#X%FexV2 zO(Y`#U}3T?;fK+GpbJoj1pJ>YCa<-zS}M)l2rfMaHR_guk|D=0u=qYRcAiFA!`C zao@MYDoq87A6c0A!rs0A5b*?D(gz{KSznUIqYZSY?U+$-KGh7AVI)35Ki~%oY=%$I z^S%VLRabRbVWS1|7|N5i!gkEMmVk*JN0fB-h;;7g?(Un`MdqZzNM)=VhP|abdo!-eGNV(H@H+ho-Ze`-Ld<@}LN8HVd>}P|XGrMi#x&U%Cj4 zA9hjWSgBQIDzTRM0^_4awX$$y6dVcoNH#xxWou^lfF7U~Ei)4n`pCo^J%Ja@O+3`( z8nuUWoNulLNuRfWN*xH`j$Zhn7FBX{#3qrKs2DG;T-Jz@zw?STv)m>u#cE zTAncU?T4SY!%m{1zE;^wUj-%7DMu7AP4M5t#bpAWH9Mfl4c#wUqD;@9CUmBAL$qGD z?zGu-H2US*-NWUjPw@m=rR5=)JC=xuP=zYuBimE*FzMl#KwjE$8U`QQ12(ug`y{bD zW*9Fpw&CF?%bcH+Q{WC5i%Sa&t$_P6I!gRQ9G42HT`;)AVTA1{$qu;8*XPr?ACG(PN^+Oz zQ-eMbhN3;JY|80_e*VS$!|AG%byTz=(vtzl*YnMY!Rw@&1~?;OXw&1r>6xIrZ!`3`AKL6A^0jcrJtsU}>8wd( ziw^GOo9Nec6jkbS$vK+ae!THtR*kX}4)G3JPFknUl82HvvEV|XRL&|3@2`xs{4`>< z-Az9sVh*>@ZUEPYiILIw@E_Nyxkper(@s~9m=)Rk>**(=x>+* z*;Nk@?^bxmFL-p5DZcfDszUJoQG*rM0`*Z{S+M(|I)jM#jA0Lr$Jv;OLLaPXBTrCk zvS(q=ZH)?3OPK#a4TiM5SFY{e^LV#}Dlw()R#Sm(U{(^C)-xYSRKxtqaT2R|gBg$FmG7{4f7ZT0-+Zy#ec}mHDT4A-|!chlOF8iMOB*-`YX9J`xkV3C-IADI7 z^G4GM`(}JfX{h&ynaL}a@)VLmzZJ4W8y$n;7>Y(Bf>FgaN28_-h8E6sVh{AMW%`V% zjv_ao`}3T6aQN?y`S7Zd+kW3`*0553pq^ith2%|hY5EozIPawS@2Y(+PDW>IzhJCJ z#e9I2TCUJdpQ*~Zu&2s>)WZJfUR@=V)^^Y;mxB0y=R-2rbw+%od?hs{m8pfH7bm;Zw{n{p z#-}}fE~QGx*hTn3pGNGL+x&oXB^fa{KaZiIt+JCa+zXzvP((TP__@BtU63hyL%OJS zqyN`ChlJA(Et=w6O_(NZfH=@%rU1g%dlvd_38JI7N>y+ZNo+_T&il8qK!60Am z^93^`D2T1-*71{d-(nM5sp$EzUs}To0OGw)X3p>-dR~pP3{`2n+^jUV$-(I@ypt?vuTz*DnWru4->?RJ^PI5)7LMWzH#s++gr=7X+J zR7u+wPeDB?cKeQ{#ZyjsJGVcnG*x=TZBc6zw9mq^o#M~O0dJ;qRrhB|7yGZ+^TAR9 zf06@Py}sDkHjW%04mpr}Q`9a4Zx{>A(R!jUZr(8$w1@fSNOA*KHVTFbv!hgY4XSWI z<`Jgjp}$!1iMT#I$lJ3CpxHwra(svy(|dEq$p&6gmasOFe=_+bD}@mz_VQ_`OtdsprsbTwJK}K!ApVlqnm@D zSjzQ@`P&aoK_7CYUNC(Bl-ljxvk0j?YT;3pWK=1Z#CeLp*v-gn3jav|FpQ_JSM(WN z$GLW8c*RvR*B7&x=p$y$+)6{VY9-OLa?uJ6`r&!qT^*(M)z{|3`F&u$iFo!%&t_eb z+xTPJZjD6^sH5O{?c#G0DJ$ZXvlD{_Dc)5cCErzXYJ4pJ(~v&CrL~mmOP*u1WnOwl z;@%oZwFmGsG>=H|jq;VUK5XVKR>5B;LRZz_jo$KNYI^{KahF<_y{Sx`(+?rjIMw5a zH77f!${@zNsGgTLadNm<+znHGk=T@mO%@kG3NI2Dpz2XE{Zs}8V+;wxuL zRdmf?)f)0cq+$AqRP@chgmV2-OdQ2tTqc(N)>DVw9OG?NV<`jE3_4G-_>+BtnX{t3FG8G=+Zn9&6I@63dsG5$)5dB*6gz3$*KZ@@rvg z=cNG(8^#uan^UCNp(P@fE<>q~BYZq;i`;q_Dtd%8;k-Q2O1(dvG8}M*6ghW?fg0O* zn}m^(@L5C0QRfSdCkl%GrMRyRNh<25=?yt++8*Id)ra9}>98XM1mzed&9L8()f4!S zYATt}--&)Lr&f-{C>q2mY-pqhAgvXJp}*fWZ2hV|%k;n(xM-(xp{AH9PX?W;j;Xt$ zc!`=>eUQ?47UzzvF`CpYy4652)}$hB!hC=Sf8;|v=BaUun4Mo{D{QZ;r z%~qJ+4bN0M=S9v!9yx&k-nSYHLo*HYK-Tth?-5(VEXBH>d8136*KBY%bk^8TuF1e}NweL=BMbMNGmc=V7W4u7?4a zE1<=ogkN0s_By;Ill&rdPyZzbh;F;S=iQ#z<4_FeO$ToyR7HN(Y~Owsuoxl)K5HMV z9@8a7*|8qLBGO31q(hq`UPg5rbH^m82E5#)zfUHevC|@-#ZCRX4u1J}pvmkB!IdKo z)N2p_5cq{kUlp-+BU1_?BvUtj0FjEI4R>eg2XpqX#zN9SUcYo#c-;7d*|FB(8)QYA zsi0cCeroa>lFZBprh}j_^m$-Y)}eDpDMP*`{Q);c3=AxOQ?pjbwU_yJq9Yc2eU1k4 zs>f*&-M>V9*#!{msSvATSsGEIFTeT3OMWi47Bw|?dt#Y@?bv<(4GsFT+gV|04L2G3 z(@wa-c-$ye+!cRyn5@3@uUhKhVw4=})PztA)Om|3@r9q8a+Od{&&)z&_jAc(z?s8l zog71Np}tbM7NQ?6RSahoz3dA0IR#$D-5_2CWj4L6qG!q!1X>^H6L6dEsmf1JoHX|N z>2&Gk@kQ{|e>tTLota+6Cisr6-;PXIB8!()@cAt_;^*N~#Z?g zb??}Vc)c09v%q*NR41?ZG}k}kJ)aSgw58;=bgnl7rZj;W$w$nG^ogsO;TTKchgWD& MRc)0TWy`St0egg5j{pDw literal 0 HcmV?d00001 diff --git a/rtabmap_demos/data/books/8.png b/rtabmap_demos/data/books/8.png new file mode 100644 index 0000000000000000000000000000000000000000..4c04a8652e0ce15815f6ab6d65657f8f5fabe2a0 GIT binary patch literal 16528 zcmXwB1yEc~v&G$6B)GFkun>Z~y9c-6!QI^@I0SbK?(Po3-8Hzo^X~Uoy{TFjsD+)| z({lRsxf`x1FM*0gfCK>nfhr{_sti0k0FMj=DBvsOmH86z1nnp+rHX)nu)3|V1^kF; zC#mTO0fE;4-vcs<0gVs>f($}RR7lk=V?9&Tl~TgxfD+FNMu?HBr%;3b^Jh)BYL8ZohJHPJw zDe8Ok&388=GEed&xe^?)!|8LcK-%^$L;Bw3=6>Po`$;Tr;iBW<%k#JPh`cHE=hTnE zkA$i#c&G45{(F(5gPA0ci+nKB$GluFUl;mC9$(Zh=WCS@Hk15=6C_^B(w=7JQ)bm$ z*Hy*k36tCSj5I~tZSf`X`n~7X7nHKDqE^h0>n3)&eDLVa8rP6*0g@9QpW5HaMG=aS zNwjYqUfHqd0@)Nco)k8}J_m+v3c!sC!;I8?-8>&BLZ2 zP2#D-o=oeOtvKAXL{#pjndy#0U%EVxzScL##O*HsOi5D$6*4)-jf;(!;~USE!e1 zRd`%1p4>UK?6qkon`jR1-$?Qm^)s?KhZDJHg+|ql8MU#~pP>qbdAkUZriEV8V94YR zM)qm98HHx;RVjyH6xVYc=(K7us+Xyi&F_4|fc4ocQk3h*LiW7KwNL_`+!%P%1W?3X){C-YO6QHyF#K7lr8qlvSG!F<7Y$K7gd$A`TK_lbTqVh zDt}$4n>?_hN3L7=YAqwMCCy~O1bN^Y;BXFF-W&C*b~(>o+JK9wTSiE%sWq*m6TyqJ?v?*4Nj! zzOF0f6i>Ch`ro?Qs8VMr$v${8stL<0(W_dt{JJnA?{=|0@b>22NIO=Wm{K}#dFSEH zmf9UPZ>g3ZH7Xd;+mQFCcM*A0krX+6B6Rt;N5{lq^~Z)HF@1W95Nt5gFP@-hzP$xq zaol_-N{%oP!)n`BDXqtvrr>#T0hZX@#IvRFh!*eTdbqt+Emdt@Ki}Q`QK%fp>Mz$jp1>7SP9}`m zYOG;g`&d{oezqhCX5zP_S;6@AHB50^n^r2`;fk~pI5NGeS@wUWwIMdNAz1NhPPeAG z+5gn7lkjQ1=G|U)f;Fjqq&xH^Y!lt&5>fALCDjvuufl2QkgqtdTDF2C*yTJsNQA$n zk1ML@9eH%1-RT`k_hsV!S@aB`b-^iH6>?;%9+-3}`(i$VS_4;(LtyxMns?)6*@~tV zy@_Qmg!+@~Dt*!m2=@&UGI>i*QpMY~LvPZw&Z8r%$f9_5q&BLsmeoBA+5+Osc77p~ zvY7`rl|Uq-T5IYvxb8d3Zo=nQ0E{7tbDf@rRn1e?f^V z(4yX{qRqt>)ScKdsP!&X^xF{aLRtLt&CnCmlB_yw_@z3M5e+@KN|2!RU3ccR)_S;_ zT8}BUXy#&B%j(FerbLxKG&IzJaRE9!v~SyqTlYUtI6gYE=+OI4k0T}Cs#o>!@bIM8 zp_lSc%?&mfC;ekAU51%Kkz$ujULwDSkKb$B0KK>c2 z_=LgyOxI7NLLHLcJu7Btcf@$+;vs>`{q6bAsOHd#TS7u&-R5C$Z|~~rYJcC9ogjBg zxFL(e2w#(qQcn;8y76wm!;mN^xRZit5PAtho!J}8^V;2@RL>{nPB=dNg~D+o@4$h4 z<8Mt}PGPms(FD1EikpB`yiHR&)d+AcjJUQN?WL;pfcLDPS$;u7Lo?8B{=8Q4AT7wK zS)txn%^u{ZCS*`uOu_8Y5pP5v?rZ)Gd(}H;O^Eu|&T!OrU70QRhLzhld2!u*D>YnU z_jhAMPYDz`u(SDowrOKyqu1%>?&Y;+(*)#|4n11#-HcT$4)ZTu=NCzSHi=Y-Nci+O zQCbynW+N7qHiG!%?07o?u{mIvax*K)3f;6(WTZHnQ(XGG`AGD3U==A@6*Oj zJM^;Ui_^#V&(G1^{2qI&AImM$uA#MeIur(k9-$_gV^a=}X<8gZ4O?-bQ(qMU9kCxF$EW*R(q?5l3&APm`J2$BZu9hNt7;HVe$ zy-z6!HuKZL!x~aqz#zE`P;=)ky_QqsV4W+~8Tzx$7A+fM_3K6bli_lK8x(Gf>>!Rk@_l*cZL{CvS8-0@cXpsDvN+o--rA zNvx7NiYf8(@m-l43T?C_^a^gg=v93$o8PjbDg{tik=^fam)cc}`Cy9r3q}Ik?~3fA zYzrsO1Q8ov{0|%72%-UtP)Q|^{j=h@*&rn%5>1W?3Y%Kw0{+Fy8ONvUlarqd)8_0s znZnabAfZm_!rG>$&}O;fS>8#YR0FXDpbSzH5+JT}5J)iujKtiZSX4&R5!|{5+TD{) z@K2-*XHM>_7g=c9I`yi6bmhjI+(TZLT|~D>S)g1t-@0zowqCh-V!PStj4KiSSr${f z;PbCt*NcswebV9M0A6^e{@vYOFaDv@uwDEQSN?3r6U&CyuC7gUc3^|8Mr^5bCwKYz zlzG#ycIw0~36{t3fjt^}@A0 zDS@uWsHFZ^NG+RI&s^G;twvE_LVX`Y$4$QwM02E%1G%GVmHJ+zR24|E#%%u006bbf z;~+pKS+{9=xWA7@DTsa(S$TYd205#H1RwbM9Nl@ejsdS^_+Ex13s2uY>dZ}??Bn49 zY+0e!ikBo`a4V6y>ve|9)zkCk@)F|PKX-n76yN|xMn<-`w?{@0EsZ<$V#M=(KOTIm zROv67wF9&Sg)lFE%hD);&Mq#Rv_4;Wx2=n7k~{WuZ<$bED=yr5yq#2ab+)!@RW0sb zEq$7Z+-+XB;W@yKgzb9Rpj-iW>s1|e(9S^-V@PvJZ@i$djrIk#1LHX8Q!L3+HzpkpoZ~ zNGUzwLk2f5U*GK2eRsXGdBqsJ4a){3@F-A2{_RCl4nlszL=CqVW)M`Ut4HxC8e;2nenP;M`@aylcCN zU?rd{DW2`Qj&&6t_MSWTceXYfCSv!fD??}ABe=$XaQgX zNeo^jY`uAdmerY2jRsQ+U>ipm3A>Rw1IZ`I4WW$f9UwQkw0%Z~=J$7m#5Yzq#!(`J zseFFpFKJ&r}m5Lgwd0}*xD7Xibf5_;aUUJEtLY8S>+0K zftS-S247KNo`6p05PM{+O94|@ef|@I3nMNv1o1Qrh~bX}KTi({oBeer1iM3mN$o$) zUY>QEZxIxGHR@%+$pBD$w6}V?eb9hKqiT@_HbyY+bBatNv^v^}tDT*lmlq25r6-Z1 z)45TNAdEXE3Uj^VL~bHb%c~YeKqYqJ0b0@WsU(+v%@pWnH8v+p& z8A`YL?g{^)rtB`t>V^Emxz_ux>eL{c<$6flBI9c1tA(YUKxa z=7i}NMts4G`};<41aL|igi29QUhkwISEMX2Jv~AldIEfW16TTtxCtZ1yd=9vwV4ww zfq~GxaloDjjQrF3qS({rf};^kWQ$cE(e6g!ARwxb`V}McAc&!hr*PWQsjc3?jA|`BYeO+kwdzGG2sboKJOo!ufen33|N{CiPWmVO( z11nNBKDbuz-QG0s*-*R9lpcDMZ7(3B=!)x2L z3h*Ct`Wiw|!hVWon)hY;Z}*LK-zmV3kfEhg+^% zJZW|ZgM*=}?+#;ui^fMezqm*lqc)|fX>H5{Px+T@@s>O*MCKGIi^FAYT5$loZpO`& zr;W(Fa(g{rZ~N!q?&3ldLsHnO!vu89#j{+ToR5pXZ;L?MxN<{Dh8paP2Yn~MRz1t} zbtmdX{btpmMN3PYK3=3()#dr{i<>jz#2w?2U>)fRDArYr>9RD5+;#c0M^4|2cdbVtpU>=z#khkl9lKs_fluNA5|{st9pL?E8Ij#@h^^^{^3HHWJ@wuJ7|y zs0F9EI(u}xzfUvih>KcGL`tgS5_zV_hzkH9sdyq1lI!a*pc&m5OJUw@cfX1Gc#Ii0 zO(5>&hp33m!;co<^!<4Iw&?|3`0QWUAueqf~YWDW@)QUeFD4)P$?tFyG zly`y?h}9jbMN!EpI_m3#2BZ3h`(ANg4rDq4UcYz(#3?!)^zWK#uety#;pEh$rQ%WR z98{%kL9qF_j02^OKl|sDvDH=W09gZ{9NFSoa-{dm%Nz(OCf*h@*-!LPBS7EY8-8Wz zUslE-3o~_hb!E?jPZoZOW-Pn`AK2r>4Vwq}y?VSx=DTTKsw@p~iHzanGP8*Rq_1Qo zLAt%(Slvcl_I*2?S>vXFza>KR$V36e4N0QO^2<^L7Cs|^fNS0I7W1#6A4%unB1E+4 zyu-sogs&e!=;|;H?Xp%|u@ms|^JBGl*L61m_~YQ<03b@hC=7##ax@7*U<0+${Nrkf z)P4ieXBR|7M6@N8r({*I?ds_8emGzE;-`IN18`A?=K}yw*L0^R#*4s(oYs!FZuY7 zy+}A1io-mSOcF@l=Ip(eQV4>xZb;xy!mbcQx390S2@3p%m@Mvf%l$;i{}6uxAj*}S z*orlaY5L0s###6TaEU;UBI^;)_xA`dKS%J*t>&&+8$BV29xVG_1(aM8$lP0i9ps52 z>{Ah>i8CRYUWcjA0}B&EknG<)HEsHom4he{*hAvNwe#FrJ%C~X^nTmhkm-TpY1!AW+CvjU~cqScn~1(#Id8>i_I4?BV5*ZQ0joTz>kbnUuIw z5nz>*GDqa0&${W;HE9Mz;jK7>yeH~3wX5%ar`EK}$HSv?E4u6n6C8xSfAIK-&ui!A zc9_xD+A2NE4)po?(+CVBfstq8KN%^R1+phKv3UktfyS0L+iT{`<%ZW6fJ}hL0Vfcf zvuq`kLFIBZ}9*i&_AP^#Z9Oit{pljAgTb1Lno*#mte((J98)8Zhd{d#uSEi zv$AED$6;hw0`}qMO}9KrqjM|vg;~c)P8rJxIoSj6X3FtP`Xaj z0@W_@p`Ep%#x56GrT>rt+B_G)S~_r%xJnXdyy@142u( z(z7y@8HtHBku|c@i+YSrSfFGI^f$)8?_^OTR|)GjHv~qmJTYI?oR1Oh54SR*x}c=C zKSGAV91-lN`YuI%?0x6`$dv=f-sTOMtclv@r7D;KSSda@`iLL#m_s&>G}ec zKGbfjcG?vHwb}fD*^(Pzr)1c0T(H0}Vzw6S{KAlDJf!*>gau;wC-jAZ71EA6Oj)K% z4M) zjAE|8bh#AUp8Gii@t?LnCw_48{X#tSPk@AefJ)s;F={i{W=z&9E{qo=u<iQ(-Y(XWIQbVhEOU{sZ8hf9UgHzd^5x}?a6 zee9fD*-KDgzrEr47vv$H-m@;6Z#U@$@$=CRzcsWjA}Jt`N zKqn*sCV_W;<~K(kshEwoBM26h#0m?Li9jBTQq>@U{~m4`AH3Hhk69W#bC3_=mh{e0 ziYFXVG2CbS;=Hn`=v<_pQ%W^7$UWU+RuX_Yc-hsAB?;$l@mi>8a)c#_^*+FX$vSFW)M2cEtG{9UgQ3pn`S+lvrNXUqeUYm*>chG<`Zbzc>{fKr{9PtZ z%AcxxG{-zaqs`F(v!)-Hi z9Z1=+xM_@@A>#OCmSaX<6zs}Y4b>P!1Zz-)AE)B6rl^s`{V}0QaBv-`De|qGh|&Ia z!AKzrf45E+2Q|mFI$GAb*OQRehlPrM=NwGUvm=^7a)wFt~UG1U_DGCZ(hx zf;ytJDUZq|gONc{h9hNVWxci2z2rMnY?>y5!pDjA*5k0AUy1g)uzBOT$b4T4~p^`}o}QJm^ZAWlqUy^aSM3V^n6+f6vcSaE7T z*xt^a*(Xfz2vrZZu(JeOJa`-LWbD3mCaE0-9cqV@F6wgzOukpS@FMCGvQ_H2zj$im z%{7qW;W%tuKLj;oOaCqj+r*ozvPj9Qq7se_!Gg-9lH4KAr%w6V+cD-Y zszmt+$i#>h=Luq9k&txyym_z_yxG)R(!F9>OMH4)&@b$mLr3}^!aWB+UQQ9ZismY{ zo68=jv%~XeD=IHc`qP?@8MHNERL4JJ;hCO3j66G24H9zX3~*FN!#g2Zd00fIs~Nm&>=woBr0Q zj)#-GXJ}0UI4$3uyLV5~7vk>-r@hfpjm4(V-4(;uEOF?0DI1>kkkbP59fLw}RE;)^ zZEbC1V`E*9kzM9Zs~cGP-Firl!(65d{0!|xVwe+z3{Nf1^JieqhXkH0&vut>0xE_8 zH!XGj_~W_vpDeLwhSyP-(x?Bq0e-@XB)wm5TRwDblz0({)n-cAI# z;_D@JW);A6O7=1d$z;@ssk0K0qd@^)ow_<;&iIhx`;-D$+7gpAW{4ehz_A9Y9WVhUS971Eq;DrLVavg%m97dM_oD}FMJ5_Fi5f2yu}=DzP`Tf1mw!&rZ)f^cq@kv z7YMKwmOBw-eCLg*&gNFSQcX1KYteX+b>&UPdQvGP6FGu(L@KN#7rRh$#F|5-ui7A3 zoQ(}nnltx6eRN#GA0I-rhQIl2$FpOV= zy9vs&5AV$7mJ>OI|EWlWtBE2|%ZX}qIR20$%sW^8+fze%virJwB5q(CavMevyX>P% zTcOD3MoNulZS7XM|`1triwlCUku0{lu4H;w@{2TtVptn!a zJyh1ehFNxMRS#cZflT>SHzBarkJ)BKj|;#GD5wp)#|Ql)krf%7jeGWz3lD2rOGFhu zT^~CG$o(+F38t6oP5(M*?ekQFh=TeFPr3+dhi~Ds!mn)=7zc5e8@EsRcJ4eU^v4Rx_(Tz%$%D z0X(wu@T5)flwNJuKt&7nG6P2%WrI6@w`|Y*nm9-GuV`WuU9=%*9wx9ljC*j(0 zJAl>(ABAIKlOt1JL0%q^Z#+FcZEusmHg|Q!giCCP6#B7+NUR%Y68IU2Q7 zDo8s4)67Fy36$|qsEN!2dK0r)u)-j2UK5}VJbVB~0f@z}&5!4kDrP1oy>_>rVYWF* zB~bNm@m*D`ON0}9^%;s1ok%?P#G5354!_d@N zK%9p7_o=cKU|0e#KMSg8YIKi_Gu#`#_I8>zb?J?v(5Okerq=?8={8`>OQLslcgN4k zqYJG9le+0-$$S9_y2RVdO|#+S^5I)xbTi1VudACo2hgwb^78mVsHh*o_03VlOY*H{ z7SSoA@RTt)aA$UUKG?9OvNWJ`6tokEop%83C>|ajkk{;q1o-(ar6YCbIe%T9Q##fH zoaN-?q!LZvHcR7N!myUca?=z$NGO7V!!nvAtu+(7wkB}?_k^DzK(PZIRXxYpIUWaI z*B&xw5vqu~vmfhC{CSH@TeMHEd1NkJYBctc#}pakgqRG(uwXvhPW^zHrufrWg6Nj7 z8UhX^z`#H=xzJ)%2W}uFj?<}07Bv#{RWOx4xS>X~s|0g+G#2BuzzF2Fx3N;6m!k#l> z3y&7;Cs;>+u;9PW3cO+SbNZbE(iCTJLo#tyO7uUu)Oi*NHKpZ485G9AeypOJ(u`Sa zvF5_nQbjoByc8)o65lXdeYVsweK^v&4#2B`=ms!eE?ocq@P6-C4-sp|ygv|fR@8ig z*`&U&asJY&(Jz25%Wl?~%`mLVv2xk!{^f0r{@D%NM?8jsClfyDsFYYKKPASxb@Zh( zi#(ln#9em1r97?fRg1jbxL+o~`F+?#UZK zGQ>-mR0dL)7Q;%c0;!MaYFNiKY6gQ!oj)yFx!^_2cq)8&8x@b2*3u(M$LnCMV&`28 z-Q`eF@k@3Tte{3k(c=Sy(+25{l``{=A))TmOPqeW3pxg+C1Zkd(DerPXP5&91VS-P zTohlY4!xgySy0=Agy`hj<;~P{szd1yS)Xzlqh6-QgQtrXJ%K^jLp)ZWh|)=dC1*q) zFwX+!N!LR7tpy4)PXXwme1Tetm*jti8X$j%k1G0Av`U!=S1o1mF8lu-6rty6)~Y~5 zM>kYnVY6T>TmXVyZWid+OEI6cgb^HQu8jJ_;-6#7U+)63N|qp7 zHar93*I;WYikqRy?1a%d_g9v{5eqI`KaH)y8=h`nY^fNZ00tch-l)s@vEAxj%7E;4 zoXg?m$Ulfp&%7yNZ)o}Utov_6jzv|;}%&-A8hesXUdL&BsF7Id&r(H zB6%EVqm=~#x1M4&GlT?7ie~b5-IuOp3k*RIH;H)^UjAAx?kUiu0;T+K4is{HxsW^q z>S2P+eC36RP{hj6Vhi^&M|A4M%Y8e}L~PgupcMibV*r-|NX9Bah42T@7T&o?aY9(a zN_YUM0UfDjhfM9bPY3q{Pp7+aN4m~Pyu#ud|KjHMR@CN~66=O%cWUzz*H8&6o9pgX zGdKc(Hr=I21UZa4%=e2k-&_iejA60s56QFGn>`u01evR>K~~8oK-^qj+WGo^?Cj(% zo}j`7+IGF&w_S8ndim3k#)tP$)4-g(H0^*(I+>X*%T zKJ636LYQA|ZTXE<8Djtis&v~^ibc(+L+{HzM5F{ttno;d&s`a*&m&})B44|4>fv6p3hSVR1Uq9ANdIJFfUK?3ywNECb`TJM2S;Ud8&rQ1i$MfRFM#q?SGqk<@ z;0Vx@vECEU)&kRwzTAG+#u-=(chUN&Gr(;2Cz z1co^h(Lu<-7AGHug6f&w#cW5c{KDGtpD1I&rYXDx^Y0xl2XvFiML6l%+1cHTckO~S zJci_+y$11Bc{AIFys^|sXLhu14S6k4hFLA!3yi=}fherT6sH3Nu%q^DsXBUkY*w_5 zRLWMA3`-<%ak@JHEtVJDfKh3Tp#Z5*alsL^InbIa>B3R{z;N3*xSEk1b{{G?MNuvT zH(VG1^X+YIDl^zLCiMS$di+`3{wI3dVGtyiM;tD%ErR*urT9x7B=$$($2ekDyW16% zYr3~c@+ZVe4Mr>K-7=-Bo2S?J_bkloB^7J)w!F-j1TupKkfvuq zx-le`O>ne?q4K^JM__;w$hl3|6g|Eo&&`i3Xv&B`TgBGi**RG5KnR6U)RTsT27$xu zQjdrobn$0#n4;aL&Hm^OMso*qOGV8Gk{^EptN^F^1FqEZR|6Z9Z2qevW|Rh?p6#y; zONS8aTLPa#WI)Xb5iCR(nnIp}VNC3Ti%4^(rac@`V?8qYyNTxVz_B?2;58*D)Qqy?t5Tf$mRu1{@<=sy1ed= z=YeF?(n7Aqojn;&SlC0yj1l?w%cJUV#O8&QyWt*`P!S5fnrLwVssJsNDCROwyN`x) zjdl0ebjQtk)QvN=1H zoM+@yeDgnJltJ15PD~|h`ARU2c~rd9<1S(fJprx;a-tZ0(tD04QTOm>br0UtF56|Sp1Ofy-RHl}Rt!1b?W)U_*vif5)_8l z$T^3YkR_RK8f}eV0IIe;~)w4g4rAZBrOH85)auj;?~bS+x}qu#fYJD`U444u;sg{vQE$6nH0E?`>D8q& zj5*T1hh46q1d$qBSI-)?fF>%&_K##E4PJQww0j)o3`dRgKbCC(&HxfOAirJa04DU| zB>K`|?SOCWYsW&S-+p(#~z|PST&=GEQdIJKql{1&LA|eH$SWtA- zL*b?|hYl7Bsb&BSKN)Bo3Uom>>^FiHp*#%d2f%N93dF3!0B2qj0bX7}_5pY{AX##0 z(@ixhq>;fR-K@>-@UW^=HFaUdpPYsDQ{XNA$78b+Z2 zc@Rq$wyPoL2=SZq7oef436?Gt*4$)j@;FhANW_^Bl#<$4pE?c{M>cDitByGpG`v9~ zXNpBFfLapg21E>T|9B@y*!=4)5?p$u7T5F>79n!&DDGJ@R>LY{@(}!n2e&Y!qmeX+ z>}8j}NX5@-9hCB!=F~X;VnINoqW+Ci>icwU(zOXRm_G?U_5%!(sK{!w`c=^-16mM! zJtAm=T!qlxWXu;&+Z}ERFlMytf|^)f99jUIfkcjAKILj4qYYb49vD+{Ek6mzH0cF|c<&>ua0j2_adwDS}2P+aa ziB0YUzZ#Q}ugj`QutQ&aQk{K|;$9enJAy64vAi^d>u68VY4Qf{*&Q=KDL@aljHLT3PTv$99q@Nr|VLxWaf6||0*w|BIoYE%9wRh81z z64>QwO6;(aPlr_(vHa-aP~R+^8PZ`OeevWgHog*ai412jsNmSBL=eKEh@&tR3pJu! znNTut*$LNmanRg)XR5#ULps33#Q|?xEh=brM4Id1G8Y)jKkUW`K>di!J9KFyg3c$7 zZd$F0sD3I~VnX=LC~^Wd*;J5zCt?F@W4=UXo=9RqZg;ys)lj7$v{DvpgyP$!?-_&T+VXH=#w$8>~FRn<)R5ILA5$Z>N z1o?0yZRiYP%aF1wf8-49lf=2P-jS)*?oBW~1csf!=ix+&7uo*^SZ zQ&2cg&MZ1E?}wp3vxQm~WQpo5pMa!6T_$A}Hen(SM2coKy{rTaDMCxla_Sx(*d!|8 zmR4&Ceo8wa>GW>{Y7fdrP>w4WsPr>#WRHQMWuR}*@CNr~uWgdedzSXkg||?=49x(o z->M^viVu+#;e{eoxy_9;?Jdz5jOF_Sglzc{s5HXP*G8#ozyIufsiCB%_?fZ@I*#QN zsZ0zCj}$5EmXT`rn%W;<^bZ`JaQGN8&Lb$tQlI~IEUl7$oj0svIL;p|R{o8{Hk6^M zmG~VwcRBFyzFz1;OcG+D1P7GlyU+Lp#)D82+Al%NaX@M=jNQ+#ibvvLLKKyjB(D&7 zFJ4YG<-kkAu!g5yR_pM$Ch2fy*pI^0p^Jo^FF!>&gl@d?K-%Wi>9B!VCly|eo9!eR zCqjBd#F_4MuFSX=lGHF{I3qmuk-CU1XzrV_(5$J^`bw%?c}!MW)ZEU$-sw-1Mt?Xa z=o;C`=k0qfFF%oct;j2~&=o22nqsS@x*v`q zbczBE?q8x zEDc_d@0S)7dyV_2v1%VTSXT72cK(eYO^?SK&gi|G+= zUCec;&9!3!*(CU}HFNl=`YTNw)oEk367HGg?y4kdHfd{kHHOp4&xBJEnr5+kA}X%J z1@dSmrJu8WSey%Ia}uI?<%V8;o_B&5ijeOxS6D)PUhHSIv7 z*{Ig4)|$6Omk<#cr)!i(fxkrp4-00xBo_T@k%T4Z{H@f~dLz^kK98)D$VWg5D?(7@ z<+2=eE}147U4hX;noUg_RlWd@qQ)aDc0EW6$DBDJd^7m2x0afT(t44P5m zUvd9Ux@i5SSkVIj5dK+iuU(R+$}IeDLLMZjOz8dH;hV@zP@@nQu@Bi=AeNkngFl{y z<(g38qQWC1^8AF zBzqet+uxIcPWSLvt4`R3NvKuV=5x6qjQn%UY=}wu7XK7~4<8;xU~HSaLMyT4kjhlr zQU_dz(ya@&eev(QUq5c zg_({ZSpPW+gw+d&pP0t7-HHE%@+avb!%+lmIS48I+a8t5=pq940EffstagsjI2a_T z6js=T$7UMHrGDHgcS~D?M1pKIQR8%2>Fq_*MjHFUeb7CT;TeW4wid%#1(p<*vKZH0 zPlJ(o6y5|>OS#U0d~~UjUD1oiaq2E^j?Jh_el;16oFHIEvyrV$2{fc!+Jro+vxM(4 zee%k}74l1EDEJ;mqJ!a?BJQ}lpcs#pnxY1RrG#SR8I~Uug}GjhnPdf=^iOyv0_!zl=*X>dL%OIxZ^ zsp~AQx;ZY-uwjK+i?U$yo{C%c{X9bK?Lf4>h4%<1P+?cw&RUg!Ld1b65hAcBVrZd7 z21MAnnuSqE*47;9v1-u}02?!>1`g5?hVr#b*-|o6F%O5Iw@UnOL>Y&j{sHwUXFbd` z-vf#sQCp2b3L6-p^ZwTO}jk&iIselZVC-*Cx@^_boA{xX^Lncj*FTv=V0Dojy=&#}q%M^=)H z>Zj;e7vRPm#00d^zNsx~)-n6{@TTiklAy{>=1Y%z{ts%c6r$8&q!S~VEIf`d>B6e% zM$*H-e?q(5yGWrv`0WfhdNljA{I7SPTu{479CRPi{Yz6)<6^9Js=?L+458XIU8 zIkYdVp{}~@bk$Ayh-u-Z3PrZefDv}B(sf7?t_i}NF|8Y6K@Glj&3+60_3Ha99YCPQ zRLJaFW#(Mw#WH}cjJ4Q{^g)_XB0m>M-L!zUzlZHSc$m_dn zNyddM)76iUx6H9ot4i6UhhXY=pJ)5$*U4$sZ8I-Btt8}tk85)gU)q%kixcM{uOEa5 zsuk)gMR;P#BVgFqmOUH3wrBI=G@z!Q^XKK{gO#~G9u%}HNh+o$&qMPwt-K^V?I4w6 zJ1@_8H-QfeApGxh0nqZjuJeT(2z?mZcXGM~FE_dHhOR!K$c&)tpCeUJfR{l?iOGvr I3L6CcA5*;osQ>@~ literal 0 HcmV?d00001 diff --git a/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py b/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py new file mode 100644 index 00000000..3432f02b --- /dev/null +++ b/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py @@ -0,0 +1,87 @@ + +# Requires installed https://github.com/chvmp/champ/tree/ros2 +# +# Example: +# 1) Launch simulator (gazebo, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos champ_sim_vslam.launch.py +# +# Note that the first time we launch gazebo, it may take a +# while to download all assets. You may need to restart the +# launch to make sure all nodes are started after the sim is ready. +# +# 2) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 launch champ_teleop teleop.launch.py +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + + sim_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_config'), 'launch', 'gazebo.launch.py'] + ) + + gz_pkg_share = FindPackageShare(package="champ_gazebo").find("champ_gazebo") + + champ_vslam = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'launch', 'champ', 'champ_vslam.launch.py'] + ) + + rviz = LaunchConfiguration('rviz').perform(context) + world = LaunchConfiguration('world').perform(context) + + return [ + TimerAction( + actions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(champ_vslam), + launch_arguments={ + 'use_sim_time': 'true', + 'rviz': rviz, + 'rtabmap_viz': LaunchConfiguration('rtabmap_viz'), + 'localization': LaunchConfiguration('localization'), + }.items() + )], period = 5.0), # Wait 5 sec to make sure simulator is ready + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(sim_launch_path), + launch_arguments={'rviz': 'false', + 'world': os.path.join(gz_pkg_share, f"worlds/{world}.world")}.items() + ), + ] + +def generate_launch_description(): + + return LaunchDescription([ + + DeclareLaunchArgument( + name='rviz', + default_value='true', + description='Run rviz' + ), + + DeclareLaunchArgument( + name='rtabmap_viz', + default_value='true', + description='Run rtabmap_viz' + ), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'world', default_value='playground', + choices=['outdoor', 'playground'], + description='Champ gazebo world.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/champ/champ_vslam.launch.py b/rtabmap_demos/launch/champ/champ_vslam.launch.py new file mode 100644 index 00000000..7b527168 --- /dev/null +++ b/rtabmap_demos/launch/champ/champ_vslam.launch.py @@ -0,0 +1,179 @@ + +# Similar to gazebo example on https://github.com/chvmp/champ/tree/ros2, we can do: +# +# Run the Gazebo environment: +# $ ros2 launch champ_config gazebo.launch.py +# +# Run Nav2's navigation and rtabmap: +# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true +# +# When a map is already created using command above, we can re-launch in localization-only mode with: +# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true localization:=true +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +def launch_setup(context, *args, **kwargs): + + localization = LaunchConfiguration('localization') + + navigation_launch_path = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py'] + ) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'champ_nav2_params.yaml'] + ) + + rviz_config_path = PathJoinSubstitution( + [FindPackageShare('champ_navigation'), 'rviz', 'navigation.rviz'] + ) + + use_sim_time = LaunchConfiguration("use_sim_time") + + # With the simulator, the imu is not published fast enough + # and have a huge delay, disabling imu usage from VO + use_imu = use_sim_time.perform(context) in ["false", "False"] + + vslam_params ={ + 'frame_id':'base_link', + 'guess_frame_id':'odom', + 'approx_sync': False, + 'use_sim_time':use_sim_time, + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'use_action_for_goal':True, + 'wait_imu_to_init': use_imu, + 'wait_for_transform': 0.5, + # RTAB-Map's parameters should be strings + 'Grid/DepthDecimation': '1', + 'Grid/RangeMax': '2', + 'GridGlobal/MinSize': '20', + 'Grid/MinClusterSize': '20', + 'Grid/MaxObstacleHeight': '2', + 'Odom/ResetCountdown': '2', # sim is very flaky + 'Kp/RoiRatios': '0.0 0.0 0.0 0.4' # ignore ground for loop closure detection (sim uses a very repetitive texture) + } + + vslam_remappings=[('imu', 'imu/data/filtered'), + ('odom', 'vo')] + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(navigation_launch_path), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_params_file + }.items() + ), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_path], + condition=IfCondition(LaunchConfiguration("rviz")), + parameters=[{'use_sim_time': use_sim_time}] + ), + + # compute imu orientation + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{ + 'use_mag':False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[ + ('imu/data_raw', 'imu/data'), + ('imu/data', 'imu/data/filtered') + ]), + + # VSLAM nodes: + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[vslam_params], + remappings=[('rgb/image', '/camera/image_raw'), + ('rgb/camera_info', '/camera/camera_info'), + ('depth/image', '/camera/depth/image_raw')]), + + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=[vslam_params, {'odom_frame_id': 'vo'}], + remappings=vslam_remappings, + arguments=["--ros-args", "--log-level", 'info']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[vslam_params], + remappings=vslam_remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[vslam_params, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=vslam_remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[vslam_params], + remappings=vslam_remappings), + + # Compute ground/obstacle clouds for nav2 voxel layers + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/depth/camera_info'), + ('cloud', '/camera/cloud')]), + + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[vslam_params], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ] + +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument( + name='use_sim_time', + default_value='false', + description='Enable use_sime_time to true' + ), + + DeclareLaunchArgument( + name='rviz', + default_value='false', + description='Run rviz' + ), + + DeclareLaunchArgument( + name='rtabmap_viz', + default_value='false', + description='Run rtabmap_viz' + ), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/find_object_demo.launch.py b/rtabmap_demos/launch/find_object_demo.launch.py new file mode 100644 index 00000000..54a63a78 --- /dev/null +++ b/rtabmap_demos/launch/find_object_demo.launch.py @@ -0,0 +1,129 @@ +# Requirements: +# find_object_2d package installed +# Download rosbag: +# * demo_find_object.db3: https://drive.google.com/file/d/1web54yQkxeGFr2UwOjKeoajGGDm0fZXT/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos find_object_demo.launch.py +# +# Rosbag: +# $ ros2 bag play demo_find_object.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans + 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP + 'Reg/Force3DoF': 'true', # 2D SLAM + } + + remappings=[ + ('rgb/image', '/camera/data_throttled_image'), + ('depth/image', '/camera/data_throttled_image_depth'), + ('rgb/camera_info', '/camera/data_throttled_camera_info'), + ('scan', '/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + config_find_object = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'find_object.ini' + ) + + data_find_object = os.path.join( + get_package_share_directory('rtabmap_demos'), 'data', 'books' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for find_object + Node( + package='image_transport', executable='republish', name='republish_rgb', output='screen', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', '/camera/data_throttled_image/compressed'), + ('out', '/camera/data_throttled_image')]), + Node( + package='image_transport', executable='republish', name='republish_depth', output='screen', + arguments=['compressedDepth', 'raw'], + remappings=[('in/compressedDepth', '/camera/data_throttled_image_depth/compressedDepth'), + ('out', '/camera/data_throttled_image_depth')]), + + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + + # Find-Object + Node( + package='find_object_2d', executable='find_object_2d', output='screen', + parameters=[{'gui': True, + 'subscribe_depth': True, + 'settings_path': config_find_object, + 'objects_path': data_find_object}], + remappings=[('rgb/image_rect_color', '/camera/data_throttled_image'), + ('depth_registered/image_raw', '/camera/data_throttled_image_depth'), + ('depth_registered/camera_info', '/camera/data_throttled_camera_info')]), + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py new file mode 100644 index 00000000..d8f8d341 --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py @@ -0,0 +1,104 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam2d.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py new file mode 100644 index 00000000..9bfc6def --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py @@ -0,0 +1,101 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d_assemble.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('use_sim_time', 'true'), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py new file mode 100644 index 00000000..5e8fab8c --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py @@ -0,0 +1,104 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_slam2d.launch.py b/rtabmap_demos/launch/husky/husky_slam2d.launch.py new file mode 100644 index 00000000..53bfcf52 --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam2d.launch.py @@ -0,0 +1,128 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam2d.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + robot_ns = LaunchConfiguration('robot_ns') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.7', # ignore laser scan points on the robot itself + 'RGBD/OptimizeMaxError':'2', + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/PointToPlaneMinComplexity':'0.04', # to be more robust to long corridors with low geometry + 'Icp/MaxTranslation': '1' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('scan', 'sensors/lidar2d_0/scan'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + 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).'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d.launch.py b/rtabmap_demos/launch/husky/husky_slam3d.launch.py new file mode 100644 index 00000000..2b36ddad --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam3d.launch.py @@ -0,0 +1,138 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam3d.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + robot_ns = LaunchConfiguration('robot_ns') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size + 'OdomF2M/ScanMaxSize': '10000' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/VoxelSize': '0.3', + 'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size + 'Icp/PointToPlaneGroundNormalsUp': '0.9', + 'Icp/RangeMin': '0.5', + 'Icp/MaxTranslation': '1' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('scan_cloud', 'sensors/lidar3d_0/points'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + 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).'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py new file mode 100644 index 00000000..62b4116f --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py @@ -0,0 +1,134 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam3d_assemble.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + robot_ns = LaunchConfiguration('robot_ns') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size + 'OdomF2M/ScanMaxSize': '10000' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_depth':False, + 'subscribe_rgb':False, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + 'topic_queue_size': 30, + 'sync_queue_size': 30, + 'approx_sync': True, + 'qos': 1, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures + 'Rtabmap/DetectionRate': '0' # Rate is limited by the assembling time below (1 Hz) + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/VoxelSize': '0.3', + 'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size + 'Icp/PointToPlaneGroundNormalsUp': '0.9', + 'Icp/RangeMin': '0.5', + 'Icp/MaxTranslation': '2' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')], + arguments=["--ros-args", "--log-level", 'warn']), + + #Assemble scans + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + namespace=robot_ns, + parameters=[{'assembling_time': 1.0, 'range_min': 0.5, 'fixed_frame_id': "", 'use_sim_time':use_sim_time, 'sync_queue_size': 30, 'topic_queue_size':30}], + remappings=remappings + [('cloud', 'sensors/lidar3d_0/points')]), + + # SLAM Mode: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'assembled_cloud')], + arguments=['-d']), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]), + ]) diff --git a/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py b/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py new file mode 100644 index 00000000..521105d1 --- /dev/null +++ b/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py @@ -0,0 +1,280 @@ +# +# Requirements: +# * Isaac simulator +# * isaac_ros_image_proc +# * isaac_ros_stereo_image_proc +# * nav2_bringup +# * isaac_ros_visual_slam (optional, for vo:=isaac) +# +# 1. Launch Isaac Simulator +# +# 2. Open Isaac Examples -> ROS2 -> Navigation -> Carter Navigation (or iw.hub Navigation, for more visual features) +# +# 3. Enable front stereo right camera: +# In the Stage tab, open World->Nova_Carter_ROS->front_hawk->right_camera_render_product, +# then under Property->Isaac Create Render Product Node->Inputs, check "Enabled". To make +# simulation faster, set height=600 and width=960. Do the same for the front stereo left camera. +# +# 4. Make sure that after you click on Play button in the simulator, you can see these topics: +# $ ros2 topic list +# /front_stereo_camera/left/camera_info +# /front_stereo_camera/left/image_raw +# /front_stereo_camera/left/image_raw/nitros_bridge +# /front_stereo_camera/right/camera_info +# /front_stereo_camera/right/image_raw +# /front_stereo_camera/right/image_raw/nitros_bridge +# /front_stereo_imu/imu +# +# 5. Launch the example: +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py +# +# 6. You should be able to send goals in RVIZ to move the robot, or use: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# === Advanced === +# With this launch file, we can also experiment with visual odometry with/without disparity computed on GPU. +# +# A. Use RTAB-Map's Visual Odometry: +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=true +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=false +# +# B. Use Isaac Visual Odometry: +# We should disable wheel odometry TF publishing in the simulator to make it work. To +# do so, in the Stage tab, open World->Nova_Carter_ROS->transform_tree_odometry->ros2_publish_raw_transform_tree, +# then under Property->ROS2Publish Raw Transform Tree Node->Inputs, change topicName from "tf" to "tf_odom_ignored". +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=true +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=false +# +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def launch_setup(context, *args, **kwargs): + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + nav2_vo_params = PathJoinSubstitution( + [pkg_rtabmap_demos, 'params', 'isaac_vslam_nav2_params.yaml']) + nav2_params = PathJoinSubstitution( + [pkg_rtabmap_demos, 'params', 'isaac_nav2_params.yaml']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'isaac', 'isaac_vslam.launch.py']) + + vo = LaunchConfiguration('vo').perform(context) + image_width = int(LaunchConfiguration('image_width').perform(context)) + image_height = int(LaunchConfiguration('image_height').perform(context)) + + left_resize_node = ComposableNode( + name='left_resize_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ResizeNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image', 'left/image_raw'), + ('camera_info', 'left/camera_info'), + ('resize/image', 'left/image_resize'), + ('resize/camera_info', 'left/camera_info_resize') + ] + ) + + right_resize_node = ComposableNode( + name='right_resize_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ResizeNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image', 'right/image_raw'), + ('camera_info', 'right/camera_info'), + ('resize/image', 'right/image_resize'), + ('resize/camera_info', 'right/camera_info_resize') + ] + ) + + left_rectify_node = ComposableNode( + name='left_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image_raw', 'left/image_resize'), + ('camera_info', 'left/camera_info_resize'), + ('image_rect', 'left/image_rect'), + ('camera_info_rect', 'left/camera_info_rect') + ] + ) + + right_rectify_node = ComposableNode( + name='right_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image_raw', 'right/image_resize'), + ('camera_info', 'right/camera_info_resize'), + ('image_rect', 'right/image_rect'), + ('camera_info_rect', 'right/camera_info_rect') + ] + ) + + disparity_node = ComposableNode( + name='disparity_node', + package='isaac_ros_stereo_image_proc', + plugin='nvidia::isaac_ros::stereo_image_proc::DisparityNode', + parameters=[{ + 'use_sim_time': True, + 'backends': 'CUDA', + 'max_disparity': 64.0 + }], + namespace="front_stereo_camera", + remappings=[ + ('left/camera_info', 'left/camera_info_rect'), + ('right/camera_info', 'right/camera_info_rect'), + ], + ) + + disparity_to_depth_node = ComposableNode( + name='disparity_to_depth_node', + package='isaac_ros_stereo_image_proc', + plugin='nvidia::isaac_ros::stereo_image_proc::DisparityToDepthNode', + parameters=[{ + 'use_sim_time': True, + }], + namespace="front_stereo_camera" + ) + + stereo_img_proc_container = ComposableNodeContainer( + name='stereo_img_proc_container', + package='rclcpp_components', + namespace="front_stereo_camera", + executable='component_container_mt', + composable_node_descriptions=[ + left_resize_node, + right_resize_node, + left_rectify_node, + right_rectify_node, + disparity_node, + disparity_to_depth_node + ], + output='screen', + arguments=['--ros-args', '--log-level', 'info', + '--log-level', 'color_format_convert:=info', + '--log-level', 'NitrosImage:=info', + '--log-level', 'NitrosNode:=info' + ], + ) + + nav2_args = [('use_sim_time', 'true')] + if vo == 'rtabmap': + # We need to change the base odom frame to vo + nav2_args.append(('params_file', nav2_vo_params)) + else: + # Use custom version with higher velocities + nav2_args.append(('params_file', nav2_params)) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=nav2_args + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('stereo_camera_namespace', 'front_stereo_camera'), + ('enable_vo', str(vo == 'rtabmap')), + ('stereo', LaunchConfiguration('stereo')) + ] + ) + + # Add actions + actions = [rtabmap, nav2, rviz, stereo_img_proc_container] + + if vo == 'isaac': + isaac_visual_slam_node = ComposableNode( + name='visual_slam_node', + package='isaac_ros_visual_slam', + plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode', + remappings=[('visual_slam/image_0', 'front_stereo_camera/left/image_rect'), + ('visual_slam/camera_info_0', 'front_stereo_camera/left/camera_info_rect'), + ('visual_slam/image_1', 'front_stereo_camera/right/image_rect'), + ('visual_slam/camera_info_1', 'front_stereo_camera/right/camera_info_rect')], + parameters=[{ + 'use_sim_time': True, + 'enable_image_denoising': True, + 'enable_planar_mode': True, + 'rectified_images': True, + 'publish_map_to_odom_tf': False, + 'odom_frame': 'odom', + 'enable_slam_visualization': True, + 'enable_observations_view': True, + 'enable_landmarks_view': True}] + ) + + isaac_vslam_container = ComposableNodeContainer( + name='isaac_visual_slam_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[isaac_visual_slam_node], + output='screen', + ) + actions.append(isaac_vslam_container) + + return actions + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('vo', default_value='none', + choices=['none', 'rtabmap', 'isaac'], description='Enable visual odometry using one of the approach. None means only wheel odometry is used. If you set this to "isaac", make sure to disable odom -> base_link if it exists, because isaac will publish on same TF!'), + DeclareLaunchArgument('stereo', default_value='true', + choices=['true', 'false'], description='Use stereo images as input instead of left+depth images.'), + DeclareLaunchArgument('image_width', default_value='960', + description='Resize input images.'), + DeclareLaunchArgument('image_height', default_value='600', + description='Resize input images.'), + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py new file mode 100644 index 00000000..6ad55bd8 --- /dev/null +++ b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py @@ -0,0 +1,139 @@ + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + +def launch_setup(context, *args, **kwargs): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + localization_value = localization.perform(context) + localization_value = localization_value == 'True' or localization_value == 'true' + enable_vo = LaunchConfiguration('enable_vo') + enable_vo_value = enable_vo.perform(context) + enable_vo_value = enable_vo_value == 'True' or enable_vo_value == 'true' + stereo = LaunchConfiguration('stereo') + stereo_value = stereo.perform(context) + stereo_value = stereo_value == 'True' or stereo_value == 'true' + rtabmap_viz = LaunchConfiguration('rtabmap_viz') + stereo_ns = LaunchConfiguration('stereo_camera_namespace').perform(context) + + parameters={ + 'frame_id':'base_link', + 'use_sim_time': use_sim_time, + 'subscribe_rgbd': True, + 'subscribe_odom': enable_vo, + 'subscribe_odom_info': enable_vo, + 'approx_sync': False, + 'use_action_for_goal':True, + 'Reg/Force3DoF':'true', + 'Vis/MinDepth': '0.2', + 'GFTT/MinDistance': '5', + 'GFTT/QualityLevel': '0.00001', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.15', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.5', # All points over 0.5 meter are ignored + 'Grid/RangeMin':'0.2', # Ignore invalid points close to camera + 'Grid/NoiseFilteringMinNeighbors':'8', # Default stereo is quite noisy, enable noise filter + 'Grid/NoiseFilteringRadius':'0.1', # Default stereo is quite noisy, enable noise filter + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + if enable_vo_value: + parameters['guess_frame_id'] = 'odom' + else: + parameters['odom_frame_id'] = 'odom' + + arguments = [] + if localization_value: + parameters['Mem/IncrementalMemory'] = 'True' + parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + remappings=[('rgbd_image', '/'+stereo_ns+'/rgbd_image'), + ('map', '/map')] + vo_node_prefix = 'rgbd' + if stereo_value: + vo_node_prefix = 'stereo' + + return [ + # Sync image data together + Node( + condition=UnlessCondition(stereo), + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=stereo_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=[ + ('rgb/image', 'left/image_rect'), + ('rgb/camera_info', 'left/camera_info_rect'), + ('depth/image', 'depth')]), + + Node( + condition=IfCondition(stereo), + package='rtabmap_sync', executable='stereo_sync', output='screen', + namespace=stereo_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('left/camera_info', 'left/camera_info_rect'), + ('right/image_rect', 'right/image_rect'), + ('right/camera_info', 'right/camera_info_rect')]), + + Node( + condition=IfCondition(enable_vo), + package='rtabmap_odom', executable=vo_node_prefix+'_odometry', output='screen', + namespace='rtabmap', + parameters=[parameters, {'odom_frame_id': 'vo'}], + remappings=remappings), + + # VSLAM: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace='rtabmap', + parameters=[parameters], + remappings=remappings, + arguments=arguments), + + # Visualization: + Node( + condition=IfCondition(rtabmap_viz), + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace='rtabmap', + parameters=[parameters], + remappings=remappings), + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'enable_vo', default_value='false', + description='Enable RTAB-Map\'s visual odometry.'), + + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', + description='Launch rtabmap_viz for visualization.'), + + DeclareLaunchArgument( + 'stereo', default_value='false', + description='Use stereo images as input instead of left+depth images.'), + + DeclareLaunchArgument( + 'stereo_camera_namespace', default_value='front_stereo_camera', + description='Namespace of the stereo camera.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/multisession_mapping_demo.launch.py b/rtabmap_demos/launch/multisession_mapping_demo.launch.py new file mode 100644 index 00000000..b5841039 --- /dev/null +++ b/rtabmap_demos/launch/multisession_mapping_demo.launch.py @@ -0,0 +1,115 @@ +# Requirements: +# Download one or more rosbags: +# * map1.db3: https://drive.google.com/file/d/1XajzWm0u1Tk7m7x63ybcKVMXj80r5P6r/view?usp=drive_link +# * map2.db3: https://drive.google.com/file/d/1_FxEalE2O-DQKq2tRpLIpDn5Mbvu0jZc/view?usp=drive_link +# * map3.db3: https://drive.google.com/file/d/1dJzMOoRPA28gQZUIWCeAa08Qn4wG9oRw/view?usp=drive_link +# * map4.db3: https://drive.google.com/file/d/19Y6yye0ndIIwdhEWMwTdoiSy9WlKS44c/view?usp=drive_link +# * map5.db3: https://drive.google.com/file/d/1zCx4Q4SftPplQtW1xeG-W3OkbTxwd5GD/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ rm ~/.ros/rtabmap.db +# $ ros2 launch rtabmap_demos multisession_mapping_demo.launch.py +# +# Rosbag: +# $ ros2 bag play map1.db3 --clock +# when done, you can play the next bag(s): +# $ ros2 bag play map2.db3 --clock +# $ ros2 bag play map3.db3 --clock +# $ ros2 bag play map4.db3 --clock +# $ ros2 bag play map5.db3 --clock +# +# Refer to this paper for more info: https://arxiv.org/abs/2407.15305 +# +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'false', + 'RGBD/ProximityBySpace': 'false', # Referred paper did only global loop closure detection + 'RGBD/OptimizeFromGraphEnd': 'true', + 'Reg/Strategy': '1', + 'Icp/Iterations': '30', + 'Icp/VoxelSize': '0', + 'Vis/MinInliers': '12', + 'Vis/MaxDepth': '0', + 'RGBD/AngularUpdate': '0.01', + 'RGBD/LinearUpdate': '0.01', + 'Rtabmap/TimeThr': '700', + 'Mem/RehearsalSimilarity': '0.30', # Referred paper used 0.45 with SURF, here with SIFT, we will use 0.3 + 'Kp/TfIdfLikelihoodUsed': 'false', + 'Bayes/FullPredictionUpdate': 'true', + 'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries + 'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries + 'Kp/MaxFeatures': '400', + 'Reg/Force3DoF': 'true', + 'RGBD/OptimizeMaxError': '10', + 'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2) + 'Optimizer/Iterations': '100', + 'Kp/IncrementalFlann': 'false', # Referred paper didn't use incremental FLANN + 'Icp/MaxTranslation': '0.5', + } + + remappings=[ + ('rgb/image', '/data_throttled_image'), + ('depth/image', '/data_throttled_image_depth'), + ('rgb/camera_info', '/data_throttled_camera_info'), + ('scan', '/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'rgb_image_transport':'compressed', + 'depth_image_transport':'compressedDepth', + 'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM node: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_demos/launch/robot_mapping_demo.launch.py b/rtabmap_demos/launch/robot_mapping_demo.launch.py new file mode 100644 index 00000000..d6ed7451 --- /dev/null +++ b/rtabmap_demos/launch/robot_mapping_demo.launch.py @@ -0,0 +1,112 @@ +# Requirements: +# Download rosbag: +# * demo_mapping.db3: https://drive.google.com/file/d/1v9qJ2U7GlYhqBJr7OQHWbDSCfgiVaLWb/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play demo_mapping.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans + 'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM + 'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM + 'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together. + 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP + 'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure + 'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated + 'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map + 'Reg/Force3DoF': 'true', # 2D SLAM + 'Grid/FromDepth': 'false', # Create 2D occupancy grid from laser scan + 'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations + 'RGBD/LocalRadius': '5', # limit length of proximity detections + 'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure + 'Icp/PM': 'false', + 'Icp/PointToPlane': 'false', + 'Icp/MaxCorrespondenceDistance': '0.15', + 'Icp/VoxelSize': '0.05' + } + + remappings=[ + ('rgb/image', '/data_throttled_image'), + ('depth/image', '/data_throttled_image_depth'), + ('rgb/camera_info', '/data_throttled_camera_info'), + ('scan', '/jn0/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'rgb_image_transport':'compressed', + 'depth_image_transport':'compressedDepth', + 'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py new file mode 100644 index 00000000..41036389 --- /dev/null +++ b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py @@ -0,0 +1,155 @@ +# Requirements: +# Download one or both rosbags: +# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link +# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play stereo_outdoorA.db3 --clock +# when done, you can play the secon bag: +# $ ros2 bag play stereo_outdoorB.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node, SetParameter, SetRemap +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + pkg_stereo_image_proc = get_package_share_directory( + 'stereo_image_proc') + + # Paths + stereo_image_proc_launch = PathJoinSubstitution( + [pkg_stereo_image_proc, 'launch', 'stereo_image_proc.launch.py']) + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'subscribe_rgbd':True, + 'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs + 'map_negative_poses_ignored':True, + 'subscribe_odom_info': True, + # RTAB-Map's internal parameters should be strings + 'OdomF2M/MaxSize': '1000', + 'GFTT/MinDistance': '10', + 'GFTT/QualityLevel': '0.00001', + #'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + #'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + } + + remappings=[ + ('rgbd_image', '/stereo_camera/rgbd_image'), + ('odom', '/vo')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc + Node( + package='image_transport', executable='republish', name='republish_left', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'left/image_raw_throttle/compressed'), + ('out', 'left/image_raw')]), + Node( + package='image_transport', executable='republish', name='republish_right', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'right/image_raw_throttle/compressed'), + ('out', 'right/image_raw')]), + + # Run the ROS package stereo_image_proc for image rectification + GroupAction( + actions=[ + + SetRemap(src='camera_info',dst='camera_info_throttle'), + SetRemap(src='camera_info',dst='camera_info_throttle'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([stereo_image_proc_launch]), + launch_arguments=[ + ('left_namespace', 'stereo_camera/left'), + ('right_namespace', 'stereo_camera/right'), + ('disparity_range', '128'), + ] + ), + ] + ), + + # Synchronize stereo data together in a single topic + # Issue: stereo_img_proc doesn't produce color and + # grayscale images exactly the same (there is a small + # vertical shift with color), we should use grayscale for + # left and right images to get similar results than on ros1 noetic. + Node( + package='rtabmap_sync', executable='stereo_sync', output='screen', + namespace='stereo_camera', + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('right/image_rect', 'right/image_rect'), + ('left/camera_info', 'left/camera_info_throttle'), + ('right/camera_info', 'right/camera_info_throttle')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[parameters], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) + + + + + diff --git a/rtabmap_demos/launch/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py similarity index 57% rename from rtabmap_demos/launch/turtlebot3_rgbd.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index 93f3d97d..b9076449 100644 --- a/rtabmap_demos/launch/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -1,34 +1,15 @@ -# Requirements: -# Install Turtlebot3 packages -# Modify turtlebot3_waffle SDF: -# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf -# 2) Add -# -# camera_rgb_frame -# camera_rgb_optical_frame -# 0 0 0 -1.57079632679 0 -1.57079632679 -# -# 0 0 1 -# -# -# 3) Rename to -# 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 -# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: @@ -43,7 +24,6 @@ from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') parameters={ @@ -51,9 +31,13 @@ def generate_launch_description(): 'use_sim_time':use_sim_time, 'subscribe_depth':True, 'use_action_for_goal':True, - 'qos_image':qos, - 'qos_imu':qos, 'Reg/Force3DoF':'true', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -69,10 +53,6 @@ def generate_launch_description(): 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true'), - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), @@ -100,4 +80,22 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[parameters], remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), ]) diff --git a/rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py similarity index 56% rename from rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py index e1034ab7..733330fc 100644 --- a/rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py @@ -1,34 +1,15 @@ -# Requirements: -# Install Turtlebot3 packages -# Modify turtlebot3_waffle SDF: -# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf -# 2) Add -# -# camera_rgb_frame -# camera_rgb_optical_frame -# 0 0 0 -1.57079632679 0 -1.57079632679 -# -# 0 0 1 -# -# -# 3) Rename to -# 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 -# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_rgbd_sync.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_rgbd_scan.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: @@ -44,7 +25,6 @@ from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') parameters={ @@ -53,13 +33,17 @@ def generate_launch_description(): 'subscribe_rgbd':True, 'subscribe_scan':True, 'use_action_for_goal':True, - 'qos_scan':qos, - 'qos_image':qos, - 'qos_imu':qos, # RTAB-Map's parameters should be strings: 'Reg/Strategy':'1', 'Reg/Force3DoF':'true', 'RGBD/NeighborLinkRefining':'True', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -73,13 +57,9 @@ def generate_launch_description(): # Launch arguments DeclareLaunchArgument( - 'use_sim_time', default_value='true', + 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - + DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), @@ -87,7 +67,7 @@ def generate_launch_description(): # Nodes to launch Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', - parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}], + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], remappings=remappings), # SLAM Mode: @@ -111,4 +91,22 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[parameters], remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), ]) diff --git a/rtabmap_demos/launch/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py similarity index 54% rename from rtabmap_demos/launch/turtlebot3_scan.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py index 721180a9..e027c7e0 100644 --- a/rtabmap_demos/launch/turtlebot3_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py @@ -1,36 +1,32 @@ -# Requirements: -# Install Turtlebot3 packages -# Note that we can edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot_waffle/model.sdf -# to increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: # $ ros2 run turtlebot3_teleop teleop_keyboard from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node -def generate_launch_description(): - +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') - localization = LaunchConfiguration('localization') + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'True' or localization == 'true' + icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' parameters={ 'frame_id':'base_footprint', @@ -40,18 +36,51 @@ def generate_launch_description(): 'subscribe_scan':True, 'approx_sync':True, 'use_action_for_goal':True, - 'qos_scan':qos, - 'qos_imu':qos, 'Reg/Strategy':'1', 'Reg/Force3DoF':'true', 'RGBD/NeighborLinkRefining':'True', 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } - + arguments = [] + if localization: + parameters['Mem/IncrementalMemory'] = 'False' + parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + remappings=[ ('scan', '/scan')] + if icp_odometry: + remappings.append(('odom', 'icp_odom')) + + return [ + # Nodes to launch + + # ICP odometry (optional) + Node( + condition=IfCondition(LaunchConfiguration('icp_odometry')), + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[parameters, + {'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom'}], + remappings=remappings), + + # SLAM: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=arguments), + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + ] + +def generate_launch_description(): return LaunchDescription([ # Launch arguments @@ -59,35 +88,13 @@ def generate_launch_description(): 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true'), - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), - - # Nodes to launch - # SLAM mode: - Node( - condition=UnlessCondition(localization), - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[parameters], - remappings=remappings, - arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) - - # Localization mode: - Node( - condition=IfCondition(localization), - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[parameters, - {'Mem/IncrementalMemory':'False', - 'Mem/InitWMWithAllNodes':'True'}], - remappings=remappings), + DeclareLaunchArgument( + 'icp_odometry', default_value='false', + description='Launch ICP odometry on top of wheel odometry.'), - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], - remappings=remappings), + OpaqueFunction(function=launch_setup) ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py new file mode 100644 index 00000000..ad14ca21 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py @@ -0,0 +1,117 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py new file mode 100644 index 00000000..5a826cb6 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py @@ -0,0 +1,119 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans +# hitting the robot itself +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py new file mode 100644 index 00000000..284b5b68 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py @@ -0,0 +1,161 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) We can increase min scan range from 0.12 to 0.2 to avoid having scans +# hitting the robot itself +# +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world_name = LaunchConfiguration('world').perform(context) + + icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' + if icp_odometry: + # modified nav2 params to use icp_odom instead odom frame + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] + ) + else: + # original nav2 params + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] + ) + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py']) + + # To use ICP odometry, we should increase clock rate of gazebo, we copied content of + # turtlebot3_gazebo/launch/turtlebot3_world.launch here + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + f'turtlebot3_{world_name}.world' + ) + + import tempfile + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: + clock_override_file.write("---\n"+ + "gazebo:\n"+ + " ros__parameters:\n"+ + " publish_rate: 100.0") + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') + ), + launch_arguments={ + 'world': world, + 'params_file': clock_override_file.name}.items() + ) + + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') + ) + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': 'true'}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': LaunchConfiguration('x_pose'), + 'y_pose': LaunchConfiguration('y_pose') + }.items() + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gzserver_cmd, + gzclient_cmd, + robot_state_publisher_cmd, + spawn_turtlebot_cmd + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='world', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'icp_odometry', default_value='false', + description='Launch ICP odometry on top of wheel odometry.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py similarity index 95% rename from rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py rename to rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py index 3b6c79f6..965919df 100644 --- a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py @@ -4,7 +4,7 @@ # # Example: # 1) Launch simulator (turtlebot4, nav2 and rtabmap): -# $ ros2 launch rtabmap_demos turtlebot4_ignition_demo.launch.py +# $ ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py # # 2) Click on "Play" button on bottom-left of gazebo. # @@ -50,7 +50,7 @@ def generate_launch_description(): ignition_launch = PathJoinSubstitution( [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py']) rtabmap_launch = PathJoinSubstitution( - [pkg_rtabmap_demos, 'launch', 'turtlebot4_slam.launch.py']) + [pkg_rtabmap_demos, 'launch', 'turtlebot4', 'turtlebot4_slam.launch.py']) ignition = IncludeLaunchDescription( PythonLaunchDescriptionSource([ignition_launch]), @@ -68,7 +68,6 @@ def generate_launch_description(): launch_arguments=[ ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), ('localization', LaunchConfiguration('localization')), - ('qos', '2'), ('use_sim_time', 'true') ] ) diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py similarity index 89% rename from rtabmap_demos/launch/turtlebot4_slam.launch.py rename to rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py index 45600927..85558ca8 100644 --- a/rtabmap_demos/launch/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py @@ -7,9 +7,9 @@ # $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true # # 2) Launch SLAM: -# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true qos:=2 +# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true # OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" qos:=2 use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true +# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true # # 3) Click on "Play" button on bottom-left of gazebo. # @@ -33,13 +33,12 @@ from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') + rtabmap_viz = LaunchConfiguration('rtabmap_viz') icp_parameters={ 'odom_frame_id':'icp_odom', - 'guess_frame_id':'odom', - 'qos':qos + 'guess_frame_id':'odom' } rtabmap_parameters={ @@ -47,9 +46,6 @@ def generate_launch_description(): 'subscribe_scan':True, 'use_action_for_goal':True, 'odom_sensor_sync': True, - 'qos_scan':qos, - 'qos_image':qos, - 'qos_imu':qos, # RTAB-Map's parameters should be strings: 'Mem/NotLinkedNodesKept':'false' } @@ -78,18 +74,18 @@ def generate_launch_description(): 'use_sim_time', default_value='false', choices=['true', 'false'], description='Use simulation (Gazebo) clock if true'), - DeclareLaunchArgument( - 'qos', default_value='0', - description='QoS used for input sensor topics'), - DeclareLaunchArgument( 'localization', default_value='false', choices=['true', 'false'], description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', choices=['true', 'false'], + description='Launch rtabmap_viz for visualization.'), # Nodes to launch Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', - parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}], + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], remappings=remappings), Node( @@ -116,6 +112,7 @@ def generate_launch_description(): remappings=remappings), Node( + condition=IfCondition(rtabmap_viz), package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[rtabmap_parameters, shared_parameters], remappings=remappings), diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index a878ec69..151b9ea3 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -2,7 +2,7 @@ rtabmap_demos - 0.21.5 + 0.21.9 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/params/champ_nav2_params.yaml b/rtabmap_demos/params/champ_nav2_params.yaml new file mode 100644 index 00000000..6006036d --- /dev/null +++ b/rtabmap_demos/params/champ_nav2_params.yaml @@ -0,0 +1,288 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 5.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 2.0 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/isaac_nav2_params.yaml b/rtabmap_demos/params/isaac_nav2_params.yaml new file mode 100644 index 00000000..1376952e --- /dev/null +++ b/rtabmap_demos/params/isaac_nav2_params.yaml @@ -0,0 +1,295 @@ +# Isaac example: We increased max velocities. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /chassis/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.0 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.0 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [2.0, 0.0, 2.0] + min_velocity: [-2.0, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/chassis/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/rtabmap_demos/params/isaac_vslam_nav2_params.yaml b/rtabmap_demos/params/isaac_vslam_nav2_params.yaml new file mode 100644 index 00000000..0210b63e --- /dev/null +++ b/rtabmap_demos/params/isaac_vslam_nav2_params.yaml @@ -0,0 +1,295 @@ +# Isaac example: we changed the main odom_frame_id from "odom" to "vo" frame. We increased max velocities. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /chassis/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.0 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.0 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: vo + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: vo + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [2.0, 0.0, 2.0] + min_velocity: [-2.0, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/chassis/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml new file mode 100644 index 00000000..838c80b3 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml @@ -0,0 +1,287 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml new file mode 100644 index 00000000..43dce5ba --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml new file mode 100644 index 00000000..9c33bdb2 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml @@ -0,0 +1,295 @@ +# Modified to use icp_odom frame +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: icp_odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: icp_odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_examples/CMakeLists.txt b/rtabmap_examples/CMakeLists.txt index a8020127..07ea312c 100644 --- a/rtabmap_examples/CMakeLists.txt +++ b/rtabmap_examples/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_examples) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/rtabmap_examples/launch/config/euroc_left.yaml b/rtabmap_examples/config/euroc_left.yaml similarity index 100% rename from rtabmap_examples/launch/config/euroc_left.yaml rename to rtabmap_examples/config/euroc_left.yaml diff --git a/rtabmap_examples/launch/config/euroc_right.yaml b/rtabmap_examples/config/euroc_right.yaml similarity index 100% rename from rtabmap_examples/launch/config/euroc_right.yaml rename to rtabmap_examples/config/euroc_right.yaml diff --git a/rtabmap_examples/launch/config/slam_D405x2_config.rviz b/rtabmap_examples/config/slam_D405x2_config.rviz similarity index 100% rename from rtabmap_examples/launch/config/slam_D405x2_config.rviz rename to rtabmap_examples/config/slam_D405x2_config.rviz diff --git a/rtabmap_examples/launch/config/slam_D405x3_config.rviz b/rtabmap_examples/config/slam_D405x3_config.rviz similarity index 100% rename from rtabmap_examples/launch/config/slam_D405x3_config.rviz rename to rtabmap_examples/config/slam_D405x3_config.rviz diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index 01112ed9..8c9f31ca 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): # Image rectification and publishing synchronized camera_info Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_left.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_left.yaml']}], remappings=[ ('image', '/cam0/image_raw'), ('camera_info', 'left/camera_info')], @@ -96,7 +96,7 @@ def generate_launch_description(): Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_right.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_right.yaml']}], remappings=[ ('image', '/cam1/image_raw'), ('camera_info', 'right/camera_info')], diff --git a/rtabmap_examples/launch/k4a.launch.py b/rtabmap_examples/launch/k4a.launch.py index 346bf2e0..d09693a6 100644 --- a/rtabmap_examples/launch/k4a.launch.py +++ b/rtabmap_examples/launch/k4a.launch.py @@ -1,21 +1,20 @@ # Requirements: # A Kinect for Azure # Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble) +# To install Kinect SDK on Ubuntu 22.04, see https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1790#issuecomment-1531626651 +# udev rules: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/5f79890933e1c81e325633152b2f2799df825b8b/docs/usage.md#linux-device-setup # Install imu_filter_madgwick ros2 package # Example: # $ ros2 launch rtabmap_examples k4a.launch.py from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): parameters=[{ 'frame_id':'camera_base', 'subscribe_rgbd':True, - 'subscribe_odom_info':True, - 'qos':1}] + 'subscribe_odom_info':True}] remappings=[ ('imu', '/imu/data'), @@ -32,11 +31,9 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=[{ 'frame_id':'camera_base', - 'subscribe_odom_info':True, 'approx_sync':True, 'approx_sync_max_interval':0.01, 'wait_imu_to_init':True, - 'qos':1, 'queue_size':30, 'keep_color':True, # Color image needs to be rectified, diff --git a/rtabmap_examples/launch/kinect_xbox_360.launch.py b/rtabmap_examples/launch/kinect_xbox_360.launch.py index 2968e665..2b20d898 100644 --- a/rtabmap_examples/launch/kinect_xbox_360.launch.py +++ b/rtabmap_examples/launch/kinect_xbox_360.launch.py @@ -12,8 +12,7 @@ def generate_launch_description(): 'frame_id':'camera_link', 'subscribe_depth':True, 'subscribe_odom_info':True, - 'approx_sync':True, - 'qos':1}] + 'approx_sync':True}] remappings=[ ('rgb/image', '/kinect/rgb/image_raw'), diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py new file mode 100644 index 00000000..daec929f --- /dev/null +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -0,0 +1,220 @@ +# Description: +# In this example, we keep only minimal data to do LiDAR SLAM. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# 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 +# 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 topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne + +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') + + imu_topic = LaunchConfiguration('imu_topic') + imu_used = imu_topic.perform(context) != '' + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_topic_value + "/deskewed" + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == '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: + lidar_topic_deskewed = lidar_topic + + # 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': 15.0, + 'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below + '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' + } + if imu_used: + icp_odometry_parameters['wait_imu_to_init'] = True + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + # RTAB-Map's internal parameters are strings: + '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': '0.2' + } + + 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) + + remappings = [('odom', 'icp_odom')] + if imu_used: + remappings.append(('imu', LaunchConfiguration('imu_topic'))) + else: + remappings.append(('imu', 'imu_not_used')) + if rgbd_image_used: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + + nodes = [ + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)], + arguments=arguments), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) + ] + + 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)])) + + if fixed_frame_id: + # Lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2}], + remappings=[ + ('input_cloud', lidar_topic) + ]) + ) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'deskewing', default_value='true', + description='Enable lidar deskewing.'), + + 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.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='', + description='IMU topic (ignored if empty).'), + + 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( + '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( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py new file mode 100644 index 00000000..118cbcfd --- /dev/null +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -0,0 +1,226 @@ +# Description: +# In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# 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 +# 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 topic, imu topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne + +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') + + 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" + + imu_topic = LaunchConfiguration('imu_topic') + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_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' + + # 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': 15.0, + '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': True, + 'subscribe_scan_cloud': True, + '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': '0.2' + } + + remappings = [('imu', imu_topic), + ('odom', 'icp_odom')] + if rgbd_image_used: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_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) + + nodes = [ + # Lidar deskewing + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2}], + 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" + remappings=[('cloud', lidar_topic_deskewed), + ('odom', 'icp_odom')]), + + # Update the map + Node( + 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')], + 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')]) + ] + + 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.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='/imu/data', + description='Name of an IMU 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( + '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( + 'assembling_time', default_value='1.0', + description='How much time (sec) we assemble lidar scans before sending them to mapping node.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index 276673ea..ae4e2edb 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -10,8 +10,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node, SetParameter -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): parameters=[{ @@ -29,6 +30,11 @@ def generate_launch_description(): return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + # Make sure IR emitter is enabled SetParameter(name='depth_module.emitter_enabled', value=1), @@ -40,7 +46,7 @@ def generate_launch_description(): launch_arguments={'camera_namespace': '', 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '2', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), 'align_depth.enable': 'true', 'enable_sync': 'true', 'rgb_camera.profile': '640x360x30'}.items(), @@ -69,9 +75,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index 91013e21..c5284b2c 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -11,7 +11,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node, SetParameter from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): parameters=[{ @@ -29,6 +31,11 @@ def generate_launch_description(): return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), @@ -40,7 +47,7 @@ def generate_launch_description(): launch_arguments={'camera_namespace': '', 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '2', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), 'enable_infra1': 'true', 'enable_infra2': 'true', 'enable_sync': 'true'}.items(), @@ -69,9 +76,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index 944f9637..ccf865d3 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -11,7 +11,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node, SetParameter from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): parameters=[{ @@ -29,6 +31,11 @@ def generate_launch_description(): return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), @@ -40,7 +47,7 @@ def generate_launch_description(): launch_arguments={'camera_namespace': '', 'enable_gyro': 'true', 'enable_accel': 'true', - 'unite_imu_method': '2', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), 'enable_infra1': 'true', 'enable_infra2': 'true', 'enable_sync': 'true'}.items(), @@ -69,9 +76,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/rtabmap_D405x2.launch.py b/rtabmap_examples/launch/rtabmap_D405x2.launch.py index 9d1de478..7a8fda54 100644 --- a/rtabmap_examples/launch/rtabmap_D405x2.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x2.launch.py @@ -29,7 +29,7 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): config_rviz = os.path.join( - get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x2_config.rviz') + get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x2_config.rviz') rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', output='screen', diff --git a/rtabmap_examples/launch/rtabmap_D405x3.launch.py b/rtabmap_examples/launch/rtabmap_D405x3.launch.py index 168bc40d..f2d6f61f 100644 --- a/rtabmap_examples/launch/rtabmap_D405x3.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x3.launch.py @@ -31,7 +31,7 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): config_rviz = os.path.join( - get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x3_config.rviz') + get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x3_config.rviz') rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', output='screen', diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py deleted file mode 100644 index 84bdf92e..00000000 --- a/rtabmap_examples/launch/vlp16.launch.py +++ /dev/null @@ -1,127 +0,0 @@ -# Example: -# $ ros2 launch rtabmap_examples vlp16.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - -def generate_launch_description(): - - use_sim_time = LaunchConfiguration('use_sim_time') - deskewing = LaunchConfiguration('deskewing') - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'deskewing', default_value='true', - description='Enable lidar deskewing'), - - # Nodes to launch - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_driver'), 'launch'), - '/velodyne_driver_node-VLP16-launch.py']), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_pointcloud'), 'launch'), - '/velodyne_transform_node-VLP16-launch.py']), - ), - - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'deskewing':deskewing, - 'use_sim_time':use_sim_time, - # RTAB-Map's internal parameters are strings: - 'Icp/PointToPlane': 'true', - 'Icp/Iterations': '10', - 'Icp/VoxelSize': '0.1', - 'Icp/Epsilon': '0.001', - 'Icp/PointToPlaneK': '20', - 'Icp/PointToPlaneRadius': '0', - 'Icp/MaxTranslation': '2', - 'Icp/MaxCorrespondenceDistance': '1', - 'Icp/Strategy': '1', - 'Icp/OutlierRatio': '0.7', - 'Icp/CorrespondenceRatio': '0.01', - 'Odom/ScanKeyFrameThr': '0.4', - 'OdomF2M/ScanSubtractRadius': '0.1', - 'OdomF2M/ScanMaxSize': '15000', - 'OdomF2M/BundleAdjustment': 'false' - }], - remappings=[ - ('scan_cloud', '/velodyne_points') - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'wait_for_transform':0.2, - 'use_sim_time':use_sim_time, - # RTAB-Map's internal parameters are strings: - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/VoxelSize': '0.1', - 'Icp/PointToPlaneK': '20', - 'Icp/PointToPlaneRadius': '0', - 'Icp/PointToPlane': 'true', - 'Icp/Iterations': '10', - 'Icp/Epsilon': '0.001', - 'Icp/MaxTranslation': '3', - 'Icp/MaxCorrespondenceDistance': '1', - 'Icp/Strategy': '1', - 'Icp/OutlierRatio': '0.7', - 'Icp/CorrespondenceRatio': '0.2' - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'use_sim_time':use_sim_time, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), - ]) - - diff --git a/rtabmap_examples/launch/vlp16_zed.launch.py b/rtabmap_examples/launch/vlp16_zed.launch.py new file mode 100644 index 00000000..67267e1f --- /dev/null +++ b/rtabmap_examples/launch/vlp16_zed.launch.py @@ -0,0 +1,121 @@ +# Example using zed odometry for lidar deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i +# +# To use only zed's imu for deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false +# + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def launch_setup(context: LaunchContext, *args, **kwargs): + + assemble = LaunchConfiguration('assemble').perform(context) + assemble = assemble == 'true' or assemble == 'True' + + lidar3d_launch_file = 'lidar3d.launch.py' + if assemble: + lidar3d_launch_file = 'lidar3d_assemble.launch.py' + + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) + use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True' + + fixed_frame_id = '' + if use_zed_odometry: + fixed_frame_id = 'odom' + + # Hack to override grab_resolution parameter without changing any files + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'") + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_driver'), 'launch'), + '/velodyne_driver_node-VLP16-launch.py']), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_pointcloud'), 'launch'), + '/velodyne_transform_node-VLP16-launch.py']), + ), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame + 'publish_map_tf': 'false'}.items(), + ), + + # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it) + Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]), + + # Sync rgb/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync': False}], + remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered')]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('rtabmap_examples'), 'launch'), + '/', lidar3d_launch_file]), + launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'), + 'localization': LaunchConfiguration('localization'), + 'frame_id': 'zed_camera_link', + 'lidar_topic': 'velodyne_points', + 'imu_topic': '/zed/zed_node/imu/data', + 'rgbd_image_topic': 'rgbd_image', + 'fixed_frame_id': fixed_frame_id}.items()), + ] + +def generate_launch_description(): + return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + + DeclareLaunchArgument( + 'use_zed_odometry', default_value='true', + description='Use ZED\'s odometry for deskewing.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + 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( + 'assemble', default_value='false', + description='Assemble ALL lidar scans.'), + + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index ccefaee7..80ce57a5 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -55,7 +55,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'publish_map_tf': 'false'}.items(), ), - # Sync right/depth/camera_info together + # Sync rgb/depth/camera_info together Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', parameters=parameters, @@ -93,5 +93,9 @@ def generate_launch_description(): 'use_zed_odometry', default_value='false', description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + OpaqueFunction(function=launch_setup) ]) diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 1554dea5..1ad7eb81 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -2,7 +2,7 @@ rtabmap_examples - 0.21.5 + 0.21.9 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/README.md b/rtabmap_launch/README.md new file mode 100644 index 00000000..449543b6 --- /dev/null +++ b/rtabmap_launch/README.md @@ -0,0 +1,40 @@ + + +# Usage + +`rtabmap.launch` from ros1 has been ported to ROS2 as `rtabmap.launch.py` with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this: + +```bash +roslaunch zed_wrapper zed_no_tf.launch + +roslaunch rtabmap_ros rtabmap.launch \ + rtabmap_args:="--delete_db_on_start" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/rgb/camera_info \ + frame_id:=base_link \ + approx_sync:=false \ + wait_imu_to_init:=true \ + imu_topic:=/zed_node/imu/data + +``` + +The ROS2 equivalent is (using latest zed_wrapper launch file): + +```bash +ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i \ + publish_tf:=false \ + publish_map_tf:=false + +ros2 launch rtabmap_launch rtabmap.launch.py \ + rtabmap_args:="--delete_db_on_start" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/rgb/camera_info \ + frame_id:=zed_camera_link \ + approx_sync:=false \ + wait_imu_to_init:=true \ + imu_topic:=/zed/zed_node/imu/data \ + rviz:=true +``` + diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 64e70aff..26e4d549 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -174,6 +174,7 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), "config_path": LaunchConfiguration('cfg').perform(context), @@ -210,6 +211,7 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), "config_path": LaunchConfiguration('cfg').perform(context), @@ -247,6 +249,7 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "config_path": LaunchConfiguration('cfg').perform(context), "topic_queue_size": LaunchConfiguration('topic_queue_size'), @@ -431,8 +434,8 @@ def generate_launch_description(): DeclareLaunchArgument('namespace', default_value='rtabmap', description=''), DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'), DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'), - DeclareLaunchArgument('queue_size', default_value='2', description='Backward compatibility, use "sync_queue_size" instead.'), - DeclareLaunchArgument('qos', default_value='1', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), + DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'), + DeclareLaunchArgument('qos', default_value='0', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''), DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'), DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'), @@ -492,11 +495,12 @@ def generate_launch_description(): DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''), DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''), DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''), - + # imu DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'), DeclareLaunchArgument('wait_imu_to_init', default_value='false', description=''), - + DeclareLaunchArgument('always_check_imu_tf', default_value='true', description='The odometry node will always check if TF between IMU frame and base frame has changed. If false, it is checked till a valid transform is initialized.'), + # User Data DeclareLaunchArgument('subscribe_user_data', default_value='false', description='User data synchronized subscription.'), DeclareLaunchArgument('user_data_topic', default_value='/user_data', description=''), diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index fb19c7ec..8245f5be 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -2,7 +2,7 @@ rtabmap_launch - 0.21.5 + 0.21.9 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index f82a254f..581eafba 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -2,7 +2,7 @@ rtabmap_msgs - 0.21.5 + 0.21.9 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index b42ebbe0..41d730c9 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -109,6 +109,7 @@ private: protected: rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_; + void tick(const rclcpp::Time & stamp); private: rtabmap::Odometry * odometry_; @@ -178,10 +179,14 @@ private: bool compressionParallelized_; int odomStrategy_; bool waitIMUToinit_; + bool alwaysCheckImuTf_; bool imuProcessed_; - std::map imus_; + int processedMsgs_; + int droppedMsgs_; + std::map imus_; std::string configPath_; rtabmap::Transform initialPose_; + rtabmap::Transform imuLocalTransform_; rtabmap_util::ULogToRosout ulogToRosout_; @@ -189,11 +194,13 @@ private: { public: OdomStatusTask(); - void setStatus(bool isLost); + void setStatus(bool isLost, int processedMsgs, int droppedMsgs); void run(diagnostic_updater::DiagnosticStatusWrapper &stat); private: bool lost_; bool dataReceived_; + int processedMsgs_; + int droppedMsgs_; }; OdomStatusTask statusDiagnostic_; std::unique_ptr syncDiagnostic_; diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index f9d7f40b..6673b6a0 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -2,7 +2,7 @@ rtabmap_odom - 0.21.5 + 0.21.9 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index caa436a4..5eb9e0d7 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -92,7 +92,10 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), waitIMUToinit_(false), + alwaysCheckImuTf_(true), imuProcessed_(false), + processedMsgs_(0), + droppedMsgs_(0), configPath_(), initialPose_(Transform::getIdentity()), ulogToRosout_(this) @@ -113,11 +116,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o odomSensorDataFeaturesPub_ = create_publisher("odom_sensor_data/features", rclcpp::QoS(1).reliability(qos_)); odomSensorDataCompressedPub_ = create_publisher("odom_sensor_data/compressed", rclcpp::QoS(1).reliability(qos_)); - tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); + tfBuffer_ = std::make_shared(get_clock()); tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); @@ -146,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_); + alwaysCheckImuTf_ = this->declare_parameter("always_check_imu_tf", alwaysCheckImuTf_); + configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir()); if(configPath_.size() && configPath_.at(0) != '/') @@ -200,6 +201,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf = %s", alwaysCheckImuTf_?"true":"false"); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str()); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false"); } @@ -418,28 +420,20 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp); //RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds()); - rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); - if(this->frameId().compare(msg->header.frame_id) != 0) - { - localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_); - } - if(localTransform.isNull()) - { - RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f", - msg->header.frame_id.c_str(), this->frameId().c_str(), stamp); - return; - } - - IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w), - cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(), - cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z), - cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(), - cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z), - cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), - localTransform); - UScopeMutex m(imuMutex_); - imus_.insert(std::make_pair(stamp, imu)); + + if(!imuProcessed_ && imus_.empty()) + { + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_); + if(localTransform.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.", + this->frameId().c_str(), msg->header.frame_id.c_str()); + return; + } + } + + imus_.insert(std::make_pair(stamp, msg)); if(imus_.size() > 1000) { @@ -458,10 +452,12 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h dataHeaderToProcess_ = header; dataReady_.release(); dataMutex_.unlock(); + ++processedMsgs_; } else { - RCLCPP_DEBUG(get_logger(), "Dropping image/scan data"); + //RCLCPP_WARN(get_logger(), "Dropping image/scan data"); + ++droppedMsgs_; } } @@ -487,7 +483,7 @@ void OdometryROS::mainLoop() SensorData & data = dataToProcess_; std_msgs::msg::Header & header = dataHeaderToProcess_; - std::vector > imus; + std::vector > imus; { UScopeMutex m(imuMutex_); @@ -504,21 +500,58 @@ void OdometryROS::mainLoop() return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); if(iterEnd!= imus_.end()) { ++iterEnd; } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) { imus.push_back(*iter); imus_.erase(iter++); } } // end imu lock + bool imuWarnShown = false; for(size_t i=0; iframeId().compare(imus[i].second->header.frame_id) != 0) + { + // We should not have to wait for IMU TF (imu delay <<< sensor data delay), so don't + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), imus[i].second->header.frame_id, imus[i].second->header.stamp, *tfBuffer_, 0); + if(localTransform.isNull()) + { + if(imuLocalTransform_.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. All IMU msgs up to sensor data time %f are skipped! If IMU TF is not static, make sure to publish it before the the imu topic is published.", + imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp), data.stamp()); + break; + } else if(!imuWarnShown) { + imuWarnShown = true; // show only one time + RCLCPP_WARN(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. We will use latest known IMU local transform (if TF between camera/lidar and the IMU is static, you can safely ignore this warning and set always_check_imu_tf to false).", + imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp)); + } + } + else { + imuLocalTransform_ = localTransform; + } + } + else if(imuLocalTransform_.isNull()) + { + imuLocalTransform_.setIdentity(); + } + } + + IMU imu(cv::Vec4d(imus[i].second->orientation.x, imus[i].second->orientation.y, imus[i].second->orientation.z, imus[i].second->orientation.w), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->orientation_covariance.data()).clone(), + cv::Vec3d(imus[i].second->angular_velocity.x, imus[i].second->angular_velocity.y, imus[i].second->angular_velocity.z), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->angular_velocity_covariance.data()).clone(), + cv::Vec3d(imus[i].second->linear_acceleration.x, imus[i].second->linear_acceleration.y, imus[i].second->linear_acceleration.z), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->linear_acceleration_covariance.data()).clone(), + imuLocalTransform_); + + SensorData dataIMU(imu, 0, imus[i].first); odometry_->process(dataIMU); imuProcessed_ = true; } @@ -644,7 +677,7 @@ void OdometryROS::mainLoop() bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_; // process data - rclcpp::Time timeStart = now(); + rclcpp::Time timeStart = rclcpp::Clock().now(); rtabmap::OdometryInfo info; if(!groundTruth.isNull()) { @@ -1059,23 +1092,25 @@ void OdometryROS::mainLoop() { if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); } else { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); } } else // if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); + RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); } - statusDiagnostic_.setStatus(pose.isNull()); + statusDiagnostic_.setStatus(pose.isNull(), processedMsgs_, droppedMsgs_); + processedMsgs_ = 0; + droppedMsgs_ = 0; if(syncDiagnostic_.get()) { - double curentRate = 1.0/(this->now()-timeStart).seconds(); - syncDiagnostic_->tick(header.stamp, + double curentRate = 1.0/(rclcpp::Clock().now()-timeStart).seconds(); + syncDiagnostic_->tickOutput(header.stamp, maxUpdateRate_>0 ? maxUpdateRate_: expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate); @@ -1117,6 +1152,7 @@ void OdometryROS::reset(const Transform & pose) imuMutex_.lock(); imus_.clear(); imuMutex_.unlock(); + imuLocalTransform_.setNull(); this->flushCallbacks(); } @@ -1188,13 +1224,17 @@ void OdometryROS::setLogError( OdometryROS::OdomStatusTask::OdomStatusTask() : diagnostic_updater::DiagnosticTask("Odom status"), lost_(false), - dataReceived_(false) + dataReceived_(false), + processedMsgs_(0), + droppedMsgs_(0) {} -void OdometryROS::OdomStatusTask::setStatus(bool isLost) +void OdometryROS::OdomStatusTask::setStatus(bool isLost, int processedMsgs, int droppedMsgs) { dataReceived_ = true; lost_ = isLost; + processedMsgs_ += processedMsgs; + droppedMsgs_ += droppedMsgs; } void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) @@ -1211,6 +1251,18 @@ void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrappe { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking."); } + stat.add("Topics Processed", processedMsgs_); + stat.add("Topics Dropped", droppedMsgs_); + processedMsgs_ = 0; + droppedMsgs_ = 0; +} + +void OdometryROS::tick(const rclcpp::Time & stamp) +{ + if(syncDiagnostic_.get()) + { + syncDiagnostic_->tickInput(stamp); + } } } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 4476870c..d30b68aa 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -280,6 +280,9 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan scan_sub_.reset(); return; } + + tick(scanMsg->header.stamp); + scanReceived_ = true; if(this->isPaused()) { @@ -345,6 +348,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan sensor_msgs::msg::PointCloud2 scanOutDeskewed; rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); + scanOutDeskewed.header.frame_id = scanMsg->header.frame_id; scanOut = scanOutDeskewed; } else @@ -523,6 +527,9 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p cloud_sub_.reset(); return; } + + tick(pointCloudMsg->header.stamp); + cloudReceived_ = true; if(this->isPaused()) { diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 3b77e680..451973de 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -64,7 +64,7 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : approxSync6_(0), exactSync6_(0), topicQueueSize_(10), - syncQueueSize_(2), + syncQueueSize_(5), keepColor_(false) { OdometryROS::init(false, true, false); @@ -569,6 +569,8 @@ void RGBDOdometry::callback( const sensor_msgs::msg::Image::ConstSharedPtr depth, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(1); @@ -597,6 +599,8 @@ void RGBDOdometry::callback( void RGBDOdometry::callbackRGBDX( const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images) { + tick(images->header.stamp); + if(!this->isPaused()) { if(images->rgbd_images.empty()) @@ -620,6 +624,8 @@ void RGBDOdometry::callbackRGBDX( void RGBDOdometry::callbackRGBD( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(1); @@ -636,6 +642,8 @@ void RGBDOdometry::callbackRGBD2( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(2); @@ -655,6 +663,8 @@ void RGBDOdometry::callbackRGBD3( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(3); @@ -677,6 +687,8 @@ void RGBDOdometry::callbackRGBD4( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(4); @@ -702,6 +714,8 @@ void RGBDOdometry::callbackRGBD5( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(5); @@ -730,6 +744,8 @@ void RGBDOdometry::callbackRGBD6( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(6); diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index c6517eba..c649f033 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -64,7 +64,7 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : approxSync6_(0), exactSync6_(0), topicQueueSize_(10), - syncQueueSize_(2), + syncQueueSize_(5), keepColor_(false) { OdometryROS::init(true, true, false); @@ -714,6 +714,8 @@ void StereoOdometry::callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight) { + tick(imageRectLeft->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(1); @@ -744,6 +746,8 @@ void StereoOdometry::callback( void StereoOdometry::callbackRGBD( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(1); @@ -761,6 +765,8 @@ void StereoOdometry::callbackRGBD( void StereoOdometry::callbackRGBDX( const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images) { + tick(images->header.stamp); + if(!this->isPaused()) { if(images->rgbd_images.empty()) @@ -787,6 +793,8 @@ void StereoOdometry::callbackRGBD2( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(2); @@ -809,6 +817,8 @@ void StereoOdometry::callbackRGBD3( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(3); @@ -835,6 +845,8 @@ void StereoOdometry::callbackRGBD4( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(4); @@ -865,6 +877,8 @@ void StereoOdometry::callbackRGBD5( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(5); @@ -899,6 +913,8 @@ void StereoOdometry::callbackRGBD6( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(6); diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index a4e6eb39..c8f149c1 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -2,7 +2,7 @@ rtabmap_python - 0.21.5 + 0.21.9 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 316d805f..42f7c81e 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -2,7 +2,7 @@ rtabmap_ros - 0.21.5 + 0.21.9 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 29f8660a..be597c03 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -2,7 +2,7 @@ rtabmap_rviz_plugins - 0.21.5 + 0.21.9 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp index 61527917..b66b6b31 100644 --- a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp +++ b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp @@ -319,7 +319,8 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map) cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f, cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); // convert back in /base_link frame - cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); + if(!cloud->empty()) + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); } if(!cloud->empty()) diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index a9a07244..9f224f51 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -2,7 +2,7 @@ rtabmap_slam - 0.21.5 + 0.21.9 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 508804a9..73424801 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -152,10 +152,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : syncData_.valid = false; tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); @@ -248,6 +244,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance); RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "rtabmap: wait_for_transform = %f", waitForTransform_); if(this->isSubscribedToStereo()) { RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); @@ -757,9 +754,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } - auto node = rclcpp::Node::make_shared("rtabmap"); image_transport::TransportHints hints(this); - defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); + defaultSub_ = image_transport::create_subscription(this, "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); @@ -856,8 +852,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkSubOptions.callback_group = imuCallbackGroup_; imuSubOptions.callback_group = imuCallbackGroup_; - int qosGPS = 0; - int qosIMU = 0; + int qosGPS = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + int qosIMU = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions); diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 7e513323..2cafd411 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -8,6 +8,7 @@ #include "rtabmap_conversions/MsgConversion.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMutex.h" using namespace std::chrono_literals; @@ -15,14 +16,19 @@ namespace rtabmap_sync { class SyncDiagnostic { public: - SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) : + SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) : node_(node), - diagnosticUpdater_(node), - frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance), node->get_clock()), - timeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), - compositeTask_("Sync status"), - lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), - targetFrequency_(0.0), + diagnosticUpdater_(node, 2.0), + inFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&inTargetFrequency_, &inTargetFrequency_, tolerance), node->get_clock()), + inTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), + outFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&outTargetFrequency_, &outTargetFrequency_, tolerance), node->get_clock()), + outTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), + inCompositeTask_("Input Status"), + outCompositeTask_("Output Status"), + lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), + lastTickOutputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), + inTargetFrequency_(0.0), + outTargetFrequency_(0.0), windowSize_(windowSize) { UASSERT(windowSize_ >= 1); @@ -41,71 +47,120 @@ class SyncDiagnostic { // Assuming format is /back_camera/left/image, we want "back_camera" strList.pop_back(); } - compositeTask_.addTask(&frequencyStatus_); - compositeTask_.addTask(&timeStampStatus_); - diagnosticUpdater_.add(compositeTask_); + inCompositeTask_.addTask(&inFrequencyStatus_); + inCompositeTask_.addTask(&inTimeStampStatus_); + diagnosticUpdater_.add(inCompositeTask_); + outCompositeTask_.addTask(&outFrequencyStatus_); + outCompositeTask_.addTask(&outTimeStampStatus_); + diagnosticUpdater_.add(outCompositeTask_); for(size_t i=0; icreate_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); + diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } - void tick(const rclcpp::Time & stamp, double targetFrequency = 0) + void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0) { - frequencyStatus_.tick(); - timeStampStatus_.tick(stamp); - double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_; + updateFrequency( + stamp, + expectedFrequency, + inFrequencyStatus_, + inTimeStampStatus_, + inWindow_, + inTargetFrequency_, + lastTickInputStamp_); + } - window_.push_back(singlePeriod); - if(window_.size() > windowSize_) - { - window_.pop_front(); - } - double period = 0.0; - if(window_.size() == windowSize_) - { - for(size_t i=0; i0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_)) - { - targetFrequency_ = 1.0/period; - } - else if(targetFrequency>0) - { - targetFrequency_ = targetFrequency; - } - lastCallbackCalledStamp_ = rtabmap_conversions::timestampFromROS(stamp); + void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) + { + updateFrequency( + stamp, + expectedFrequency, + outFrequencyStatus_, + outTimeStampStatus_, + outWindow_, + outTargetFrequency_, + lastTickOutputStamp_); } private: void diagnosticTimerCallback() { - if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) + UScopeMutex lock(tickMutex_); + if(rtabmap_conversions::timestampFromROS(node_->now())-lastTickInputStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", topicsNotReceivedWarningMsg_.c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", topicsNotReceivedWarningMsg_.c_str()); } } + void updateFrequency( + const rclcpp::Time & stamp, + const double & expectedFrequency, + diagnostic_updater::FrequencyStatus & freqStatus, + diagnostic_updater::TimeStampStatus & timeStatus, + std::deque & window, + double & targetFrequency, + double & lastTickStamp) + { + UScopeMutex lock(tickMutex_); + + freqStatus.tick(); + timeStatus.tick(stamp); + + double stampSec = rtabmap_conversions::timestampFromROS(stamp); + double singlePeriod = stampSec - lastTickStamp; + + window.push_back(singlePeriod); + if(window.size() > windowSize_) + { + window.pop_front(); + + double period = 0.0; + if(window.size() == windowSize_) + { + for(size_t i=0; i0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) + { + targetFrequency = 1.0/period; + } + else if(expectedFrequency>0) + { + targetFrequency = expectedFrequency; + + } + } + + lastTickStamp = stampSec; + } + private: rclcpp::Node * node_; std::string topicsNotReceivedWarningMsg_; diagnostic_updater::Updater diagnosticUpdater_; - diagnostic_updater::FrequencyStatus frequencyStatus_; - diagnostic_updater::TimeStampStatus timeStampStatus_; - diagnostic_updater::CompositeDiagnosticTask compositeTask_; + diagnostic_updater::FrequencyStatus inFrequencyStatus_; + diagnostic_updater::TimeStampStatus inTimeStampStatus_; + diagnostic_updater::FrequencyStatus outFrequencyStatus_; + diagnostic_updater::TimeStampStatus outTimeStampStatus_; + diagnostic_updater::CompositeDiagnosticTask inCompositeTask_; + diagnostic_updater::CompositeDiagnosticTask outCompositeTask_; rclcpp::TimerBase::SharedPtr diagnosticTimer_; - double lastCallbackCalledStamp_; - double targetFrequency_; + double lastTickInputStamp_; + double lastTickOutputStamp_; + double inTargetFrequency_; + double outTargetFrequency_; int windowSize_; - std::deque window_; + std::deque inWindow_; + std::deque outWindow_; + UMutex tickMutex_; }; diff --git a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp index cdbcae25..429cac57 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp @@ -61,6 +61,7 @@ private: double depthScale_; int decimation_; double compressedRate_; + double approxSyncMaxInterval_; rclcpp::Time lastCompressedPublished_; diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index fb1cde35..6ff65611 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -2,7 +2,7 @@ rtabmap_sync - 0.21.5 + 0.21.9 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index c714e930..d1370c8e 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -32,7 +32,7 @@ namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : topicQueueSize_(10), - syncQueueSize_(2), + syncQueueSize_(10), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -393,7 +393,7 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : } syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_); - int qos = node.declare_parameter("qos", 0); + int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT); int qosOdom = node.declare_parameter("qos_odom", qos); int qosImage = node.declare_parameter("qos_image", qos); int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage); @@ -1104,7 +1104,7 @@ void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequen { if(syncDiagnostic_.get()) { - syncDiagnostic_->tick(stamp, targetFrequency); + syncDiagnostic_->tickOutput(stamp, targetFrequency); } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 30046ed0..2de994a5 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -35,6 +35,7 @@ void CommonDataSubscriber::depthCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -48,6 +49,7 @@ void CommonDataSubscriber::depthScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -60,6 +62,7 @@ void CommonDataSubscriber::depthScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -72,6 +75,7 @@ void CommonDataSubscriber::depthScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -88,6 +92,7 @@ void CommonDataSubscriber::depthInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -101,6 +106,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -113,6 +119,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -125,6 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null std::vector globalDescriptor; @@ -142,6 +150,7 @@ void CommonDataSubscriber::depthOdomCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -155,6 +164,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -167,6 +177,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -179,6 +190,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; @@ -195,6 +207,7 @@ void CommonDataSubscriber::depthOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -208,6 +221,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -220,6 +234,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -232,6 +247,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -249,6 +265,7 @@ void CommonDataSubscriber::depthDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -262,6 +279,7 @@ void CommonDataSubscriber::depthDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -274,6 +292,7 @@ void CommonDataSubscriber::depthDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -286,6 +305,7 @@ void CommonDataSubscriber::depthDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; @@ -302,6 +322,7 @@ void CommonDataSubscriber::depthDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -315,6 +336,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -327,6 +349,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -339,6 +362,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -356,6 +380,7 @@ void CommonDataSubscriber::depthOdomDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -369,6 +394,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -381,6 +407,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -393,6 +420,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -409,6 +437,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); @@ -422,6 +451,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); } @@ -434,6 +464,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); } @@ -446,6 +477,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) { diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp index 6edc2f49..3389ae70 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -32,6 +32,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::odomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -41,6 +42,7 @@ void CommonDataSubscriber::odomInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); @@ -50,6 +52,7 @@ void CommonDataSubscriber::odomDataCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); } @@ -58,6 +61,7 @@ void CommonDataSubscriber::odomDataInfoCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index f6a02e26..2c880ade 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -34,6 +34,7 @@ void CommonDataSubscriber::rgbCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -47,6 +48,7 @@ void CommonDataSubscriber::rgbScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -59,6 +61,7 @@ void CommonDataSubscriber::rgbScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -71,6 +74,7 @@ void CommonDataSubscriber::rgbScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -87,6 +91,7 @@ void CommonDataSubscriber::rgbInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -100,6 +105,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -112,6 +118,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -124,6 +131,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -141,6 +149,7 @@ void CommonDataSubscriber::rgbOdomCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -154,6 +163,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -166,6 +176,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -178,6 +189,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -194,6 +206,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -207,6 +220,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -219,6 +233,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -231,6 +246,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -248,6 +264,7 @@ void CommonDataSubscriber::rgbDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -261,6 +278,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -273,6 +291,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -285,6 +304,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -301,6 +321,7 @@ void CommonDataSubscriber::rgbDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -314,6 +335,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -326,6 +348,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -338,6 +361,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -355,6 +379,7 @@ void CommonDataSubscriber::rgbOdomDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -368,6 +393,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -380,6 +406,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -392,6 +419,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -408,6 +436,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -421,6 +450,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -433,6 +463,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -445,6 +476,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index 6e81e827..75a4acf6 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -36,6 +36,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::rgbdCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -61,6 +62,7 @@ void CommonDataSubscriber::rgbdScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -85,6 +87,7 @@ void CommonDataSubscriber::rgbdScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -109,6 +112,7 @@ void CommonDataSubscriber::rgbdScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -132,6 +136,7 @@ void CommonDataSubscriber::rgbdInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -158,6 +163,7 @@ void CommonDataSubscriber::rgbdOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -183,6 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -207,6 +214,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -231,6 +239,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -258,6 +267,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -284,6 +294,7 @@ void CommonDataSubscriber::rgbdDataCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -309,6 +320,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -333,6 +345,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -357,6 +370,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -384,6 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -410,6 +425,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -435,6 +451,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -459,6 +476,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -483,6 +501,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -510,6 +529,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index 5f708f43..0427997f 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(2); \ std::vector depthMsgs(2); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index e175357f..af36a52c 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(3); \ std::vector depthMsgs(3); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index edd5704f..eeb3c9b5 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(4); \ std::vector depthMsgs(4); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index e14dd915..e60cb8fd 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(5); \ std::vector depthMsgs(5); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index 320cdfb2..ff5a6016 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(6); \ std::vector depthMsgs(6); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index 02affd03..9fd8abaa 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imagesMsg->header.stamp);} \ UASSERT(!imagesMsg->rgbd_images.empty()); \ std::vector imageMsgs(imagesMsg->rgbd_images.size()); \ std::vector depthMsgs(imagesMsg->rgbd_images.size()); \ diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp index 8f8cd390..d50b40f6 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -32,6 +32,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::scan2dCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -41,6 +42,7 @@ void CommonDataSubscriber::scan2dCallback( void CommonDataSubscriber::scan3dCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -50,6 +52,7 @@ void CommonDataSubscriber::scan3dCallback( void CommonDataSubscriber::scanDescCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -59,6 +62,7 @@ void CommonDataSubscriber::scan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -68,6 +72,7 @@ void CommonDataSubscriber::scan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -77,6 +82,7 @@ void CommonDataSubscriber::scanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -86,6 +92,7 @@ void CommonDataSubscriber::odomScan2dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -95,6 +102,7 @@ void CommonDataSubscriber::odomScan3dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -104,6 +112,7 @@ void CommonDataSubscriber::odomScanDescCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -113,6 +122,7 @@ void CommonDataSubscriber::odomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -122,6 +132,7 @@ void CommonDataSubscriber::odomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -131,6 +142,7 @@ void CommonDataSubscriber::odomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -140,6 +152,7 @@ void CommonDataSubscriber::dataScan2dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -149,6 +162,7 @@ void CommonDataSubscriber::dataScan3dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -158,6 +172,7 @@ void CommonDataSubscriber::dataScanDescCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -167,6 +182,7 @@ void CommonDataSubscriber::dataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -176,6 +192,7 @@ void CommonDataSubscriber::dataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -185,6 +202,7 @@ void CommonDataSubscriber::dataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -194,6 +212,7 @@ void CommonDataSubscriber::odomDataScan2dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -203,6 +222,7 @@ void CommonDataSubscriber::odomDataScan3dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -212,6 +232,7 @@ void CommonDataSubscriber::odomDataScanDescCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -221,6 +242,7 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); } @@ -230,6 +252,7 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); } @@ -239,6 +262,7 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } #endif diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index fff29579..3aed5411 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -34,33 +34,37 @@ namespace rtabmap_sync { // SensorData void CommonDataSubscriber::sensorDataCallback( - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg) + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::sensorDataInfoCallback( - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg, + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } // SensorData + Odom void CommonDataSubscriber::sensorDataOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg) + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::sensorDataOdomInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg, + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::setupSensorDataCallbacks( diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 13ced404..d9d9ff16 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -36,6 +36,7 @@ void CommonDataSubscriber::stereoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // null @@ -50,6 +51,7 @@ void CommonDataSubscriber::stereoInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -65,6 +67,7 @@ void CommonDataSubscriber::stereoOdomCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -79,6 +82,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index eaafadf3..f129edc4 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -52,9 +52,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : exactSync_(0) { int topicQueueSize = 10; - int syncQueueSize = 2; + int syncQueueSize = 10; bool approxSync = true; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); @@ -138,7 +138,7 @@ void RGBSync::callback( const sensor_msgs::msg::Image::ConstSharedPtr image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { - syncDiagnostic_->tick(image->header.stamp); + syncDiagnostic_->tickInput(image->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp); @@ -188,6 +188,7 @@ void RGBSync::callback( stamp, rtabmap_conversions::timestampFromROS(image->header.stamp)); } } + syncDiagnostic_->tickOutput(image->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index f8b05495..56132aca 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -50,16 +50,16 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_(1.0), decimation_(1), compressedRate_(0), + approxSyncMaxInterval_(0.0), approxSyncDepth_(0), exactSyncDepth_(0) { int topicQueueSize = 10; - int syncQueueSize = 2; + int syncQueueSize = 10; bool approxSync = true; - double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); - approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); + approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_); topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); int queueSize = this->declare_parameter("queue_size", -1); if(queueSize != -1) @@ -84,7 +84,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) - RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); + RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval_); RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize); RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); @@ -99,8 +99,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : if(approxSync) { approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); - if(approxSyncMaxInterval > 0.0) - approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else @@ -109,15 +109,18 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } - image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", "raw"); + std::string depthImageTransport = this->declare_parameter("depth_image_transport", "raw"); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageSub_.getSubscriber().getTopic().c_str(), imageDepthSub_.getSubscriber().getTopic().c_str(), cameraInfoSub_.getSubscriber()->get_topic_name()); @@ -149,19 +152,20 @@ void RGBDSync::callback( const sensor_msgs::msg::Image::ConstSharedPtr depth, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { - syncDiagnostic_->tick(image->header.stamp); + syncDiagnostic_->tickInput(image->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp); double depthStamp = rtabmap_conversions::timestampFromROS(depth->header.stamp); double stampDiff = fabs(rgbStamp - depthStamp); - if(stampDiff > 0.010) + if(stampDiff > 0.010 && approxSyncMaxInterval_ == 0.0) { RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " "to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use " - "approx_sync=false if streams have all the exact same timestamp.", + "approx_sync=false if streams have all the exact same timestamp. Setting approx_sync_max_interval " + "will suppress this warning.", stampDiff, rgbStamp, depthStamp); @@ -273,6 +277,7 @@ void RGBDSync::callback( depthStamp, rtabmap_conversions::timestampFromROS(depth->header.stamp)); } } + syncDiagnostic_->tickOutput(image->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index c5d150e0..e1ada274 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -43,11 +43,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : SYNC_INIT(rgbd8) { int topicQueueSize = 10; - int syncQueueSize = 2; + int syncQueueSize = 10; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); @@ -176,13 +176,14 @@ void RGBDXSync::rgbd2Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(2); output.rgbd_images[0]=(*image0); output.rgbd_images[1]=(*image1); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd3Callback( @@ -190,7 +191,7 @@ void RGBDXSync::rgbd3Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(3); @@ -198,6 +199,7 @@ void RGBDXSync::rgbd3Callback( output.rgbd_images[1]=(*image1); output.rgbd_images[2]=(*image2); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd4Callback( @@ -206,7 +208,7 @@ void RGBDXSync::rgbd4Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(4); @@ -215,6 +217,7 @@ void RGBDXSync::rgbd4Callback( output.rgbd_images[2]=(*image2); output.rgbd_images[3]=(*image3); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd5Callback( @@ -224,7 +227,7 @@ void RGBDXSync::rgbd5Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(5); @@ -234,6 +237,7 @@ void RGBDXSync::rgbd5Callback( output.rgbd_images[3]=(*image3); output.rgbd_images[4]=(*image4); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd6Callback( @@ -244,7 +248,7 @@ void RGBDXSync::rgbd6Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(6); @@ -255,6 +259,7 @@ void RGBDXSync::rgbd6Callback( output.rgbd_images[4]=(*image4); output.rgbd_images[5]=(*image5); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd7Callback( @@ -266,7 +271,7 @@ void RGBDXSync::rgbd7Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(7); @@ -278,6 +283,7 @@ void RGBDXSync::rgbd7Callback( output.rgbd_images[5]=(*image5); output.rgbd_images[6]=(*image6); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd8Callback( @@ -290,7 +296,7 @@ void RGBDXSync::rgbd8Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(8); @@ -303,6 +309,7 @@ void RGBDXSync::rgbd8Callback( output.rgbd_images[6]=(*image6); output.rgbd_images[7]=(*image7); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index 1978731d..f9408406 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -51,10 +51,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : exactSync_(0) { int topicQueueSize = 10; - int syncQueueSize = 2; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); @@ -139,7 +139,7 @@ void StereoSync::callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight) { - syncDiagnostic_->tick(imageLeft->header.stamp); + syncDiagnostic_->tickInput(imageLeft->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp); @@ -209,6 +209,7 @@ void StereoSync::callback( rightStamp, rtabmap_conversions::timestampFromROS(imageRight->header.stamp)); } } + syncDiagnostic_->tickOutput(imageLeft->header.stamp); } } diff --git a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp index 83abad34..dbad53a6 100644 --- a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp +++ b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp @@ -57,8 +57,8 @@ private: const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg); private: - image_transport::CameraPublisher depthImage16Pub_; - image_transport::CameraPublisher depthImage32Pub_; + image_transport::Publisher depthImage16Pub_; + image_transport::Publisher depthImage32Pub_; rclcpp::Publisher::SharedPtr cameraInfo16Pub_; rclcpp::Publisher::SharedPtr cameraInfo32Pub_; rclcpp::Publisher::SharedPtr pointCloudTransformedPub_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 1ffb918d..60e64caa 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -2,7 +2,7 @@ rtabmap_util - 0.21.5 + 0.21.9 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/src/PointCloudAssemblerNode.cpp b/rtabmap_util/src/PointCloudAssemblerNode.cpp index 0c16a43a..3c54b469 100644 --- a/rtabmap_util/src/PointCloudAssemblerNode.cpp +++ b/rtabmap_util/src/PointCloudAssemblerNode.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. int main(int argc, char **argv) { + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); rclcpp::init(argc, argv); rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); diff --git a/rtabmap_util/src/PointCloudToDepthImageNode.cpp b/rtabmap_util/src/PointCloudToDepthImageNode.cpp index bb889f2e..0a4f081e 100644 --- a/rtabmap_util/src/PointCloudToDepthImageNode.cpp +++ b/rtabmap_util/src/PointCloudToDepthImageNode.cpp @@ -25,10 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap_util/pointcloud_to_depthimage.hpp" int main(int argc, char **argv) { + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); rclcpp::init(argc, argv); rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index be2448af..d5b896b6 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -42,7 +42,7 @@ namespace rtabmap_util DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : rclcpp::Node("disparity_to_depth", options) { - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index c079c8ea..5de875e8 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -42,7 +42,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) : tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_); qos = this->declare_parameter("qos", qos); diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index 44eaf570..2ebd00a7 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -14,14 +14,10 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) : slerp_(false) { tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); int queueSize = 5; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; queueSize = this->declare_parameter("queue_size", queueSize); qos = this->declare_parameter("qos", qos); fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); @@ -77,6 +73,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared sensor_msgs::msg::PointCloud2 scanOutDeskewed; rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); + scanOutDeskewed.header.frame_id = msg->header.frame_id; pubScan_->publish(scanOutDeskewed); } diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index c9a99db0..0f697caf 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -55,7 +55,7 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : frameId_ = this->declare_parameter("frame_id", frameId_); mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_); waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index ad0c890d..27401b55 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -64,7 +64,7 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) int count = 2; bool approx=true; double approxSyncMaxInterval = 0.0; - int qos=0; + int qos=RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); int queueSize = this->declare_parameter("queue_size", -1); if(queueSize != -1) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index fa1b95d8..688a634a 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -73,9 +73,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); - int topicQueueSize = 1; - int syncQueueSize = 5; - int qos = 0; + int topicQueueSize = 10; + int syncQueueSize = 10; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool subscribeOdomInfo = false; topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index fa96c1ad..9cd7bde8 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -68,7 +68,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : { int topicQueueSize = 1; int syncQueueSize = 10; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index 01a1fb29..fb5bd560 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -75,7 +75,7 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : std::string roiStr; int topicQueueSize = 1; int syncQueueSize = 10; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 0443c0b6..869b922b 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -60,9 +60,9 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); - int topicQueueSize = 1; + int topicQueueSize = 10; int syncQueueSize = 10; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool approx = true; topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); int queueSize = this->declare_parameter("queue_size", -1); @@ -107,8 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_); RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_); - depthImage16Pub_ = image_transport::create_camera_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm - depthImage32Pub_ = image_transport::create_camera_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters + depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm + depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters pointCloudTransformedPub_ = create_publisher("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cameraInfo16Pub_ = create_publisher(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); cameraInfo32Pub_ = create_publisher(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); @@ -159,6 +159,10 @@ void PointCloudToDepthImage::callback( if(cloudDisplacement.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!", + pointCloud2Msg->header.frame_id.c_str(), + cameraInfoMsg->header.frame_id.c_str(), + fixedFrameId_.c_str()); return; } @@ -171,6 +175,9 @@ void PointCloudToDepthImage::callback( if(cloudToCamera.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!", + pointCloud2Msg->header.frame_id.c_str(), + cameraInfoMsg->header.frame_id.c_str()); return; } rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera; @@ -239,7 +246,7 @@ void PointCloudToDepthImage::callback( if(depthImage32Pub_.getNumSubscribers()) { depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg); + depthImage32Pub_.publish(depthImage.toImageMsg()); if(cameraInfo32Pub_->get_subscription_count()) { cameraInfo32Pub_->publish(cameraInfoMsgOut); @@ -250,7 +257,7 @@ void PointCloudToDepthImage::callback( { depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image); - depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg); + depthImage16Pub_.publish(depthImage.toImageMsg()); if(cameraInfo16Pub_->get_subscription_count()) { cameraInfo16Pub_->publish(cameraInfoMsgOut); diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index ce5d8e57..45749ccd 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -52,7 +52,7 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) : compress_(false), uncompress_(false) { - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); compress_ = this->declare_parameter("compress", compress_); uncompress_ = this->declare_parameter("uncompress", uncompress_); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index c0a9fc2b..d1b85f97 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -39,7 +39,7 @@ namespace rtabmap_util RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : Node("rgbd_split", options) { - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index a00d3fc9..887d1d87 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -2,7 +2,7 @@ rtabmap_viz - 0.21.5 + 0.21.9 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index fdb3cd87..0da4ef78 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -75,10 +75,6 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : maxOdomUpdateRate_(10) { tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; @@ -548,8 +544,10 @@ void GuiWrapper::commonMultiCameraCallback( std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -583,9 +581,18 @@ void GuiWrapper::commonMultiCameraCallback( odomHeader = imageMsgs[0]->header; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -726,7 +733,7 @@ void GuiWrapper::commonMultiCameraCallback( cameraModels, 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -749,8 +756,10 @@ void GuiWrapper::commonStereoCallback( { std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -776,9 +785,18 @@ void GuiWrapper::commonStereoCallback( odomHeader = leftCamInfoMsg.header; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -906,7 +924,7 @@ void GuiWrapper::commonStereoCallback( stereoModel, 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -922,8 +940,10 @@ void GuiWrapper::commonLaserScanCallback( { std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -949,9 +969,18 @@ void GuiWrapper::commonLaserScanCallback( return; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -1053,7 +1082,7 @@ void GuiWrapper::commonLaserScanCallback( rtabmap::CameraModel(), 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -1068,21 +1097,20 @@ void GuiWrapper::commonOdomCallback( std_msgs::msg::Header odomHeader = odomMsg->header; - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, odomMsg->child_frame_id, odomHeader.stamp, *tfBuffer_, waitForTransform_); + Transform odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) + + UASSERT(odomMsg->twist.covariance.size() == 36); + if(odomMsg->twist.covariance[0] != 0 && + odomMsg->twist.covariance[7] != 0 && + odomMsg->twist.covariance[14] != 0 && + odomMsg->twist.covariance[21] != 0 && + odomMsg->twist.covariance[28] != 0 && + odomMsg->twist.covariance[35] != 0) { - UASSERT(odomMsg->twist.covariance.size() == 36); - if(odomMsg->twist.covariance[0] != 0 && - odomMsg->twist.covariance[7] != 0 && - odomMsg->twist.covariance[14] != 0 && - odomMsg->twist.covariance[21] != 0 && - odomMsg->twist.covariance[28] != 0 && - odomMsg->twist.covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); - } + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); } + if(odomHeader.frame_id.empty()) { RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?"); @@ -1125,7 +1153,7 @@ void GuiWrapper::commonOdomCallback( rtabmap::CameraModel(), 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -1139,8 +1167,10 @@ void GuiWrapper::commonSensorDataCallback( UASSERT(sensorDataMsg.get()); std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -1155,9 +1185,18 @@ void GuiWrapper::commonSensorDataCallback( { odomHeader = sensorDataMsg->header; odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -1228,7 +1267,7 @@ void GuiWrapper::commonSensorDataCallback( info.reg.covariance = covariance; rtabmap::OdometryEvent odomEvent( data, - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index d3e64897..cecd841e 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -85,8 +85,7 @@ QString PreferencesDialogROS::getParamMessage() bool PreferencesDialogROS::hasAllParameters() { - auto node = std::make_shared("rtabmap_viz"); - auto client = std::make_shared(node, rtabmapNodeName_); + auto client = std::make_shared(node_, rtabmapNodeName_); return client->service_is_ready(); } @@ -98,8 +97,13 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) path = filePath; } - auto node = std::make_shared("rtabmap_viz"); - RCLCPP_INFO(node->get_logger(), "%s", this->getParamMessage().toStdString().c_str()); + char nodeName[42]; + snprintf( + nodeName, sizeof(nodeName), "rtabmap_viz_param_client_%zx", + reinterpret_cast(this) + ); + auto node = std::make_shared(nodeName); + RCLCPP_INFO(node_->get_logger(), "%s", this->getParamMessage().toStdString().c_str()); rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); // remove Odom parameters for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();) @@ -145,7 +149,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) auto client = std::make_shared(node, rtabmapNodeName_); if (!client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(node->get_logger(), "Can't call rtabmap parameters service, is the node running?"); + RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?"); } int readCount = 0; if(client->service_is_ready()) @@ -163,18 +167,18 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) } } - RCLCPP_INFO(node->get_logger(), "Parameters read = %d", readCount); + RCLCPP_INFO(node_->get_logger(), "Parameters read = %d", readCount); if(readCount>0) { - RCLCPP_INFO(node->get_logger(), "Parameters successfully read."); + RCLCPP_INFO(node_->get_logger(), "Parameters successfully read."); } else { if(this->isVisible()) { QString warning = tr("Failed to get RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); - RCLCPP_WARN(node->get_logger(), "%s", warning.toStdString().c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", warning.toStdString().c_str()); QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); } return false; From 5c81b95c5995bf3be9c20439774adf999d0f8982 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 30 Nov 2024 17:51:16 -0800 Subject: [PATCH 062/126] Updated ros2 CI workflows --- .github/workflows/docker-ros2.yml | 16 ++++------------ .github/workflows/ros2.yml | 11 ++++------- 2 files changed, 8 insertions(+), 19 deletions(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 60e114c3..f7069e80 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -11,7 +11,7 @@ jobs: strategy: matrix: - docker_tag: [humble, humble-latest, iron, iron-latest, jazzy-latest] + docker_tag: [humble, humble-latest, jazzy, jazzy-latest] include: - docker_tag: humble docker_path: 'humble' @@ -22,19 +22,11 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 - - docker_tag: iron - docker_path: 'iron' + - docker_tag: jazzy + docker_path: 'jazzy' docker_platforms: | linux/amd64 - - docker_tag: iron-latest - docker_path: 'iron/latest' - docker_platforms: | - linux/amd64 -# Re-add "jazzy" after binaries are released -# - docker_tag: jazzy -# docker_path: 'jazzy' -# docker_platforms: | -# linux/amd64 + linux/arm64 - docker_tag: jazzy-latest docker_path: 'jazzy/latest' docker_platforms: | diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 3bac8867..ad83b0a8 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -3,7 +3,7 @@ name: ros2 on: push: - branches: [ ros2 ] + branches: [ ros2, humble-devel, jazzy-devel] pull_request: branches: [ ros2 ] @@ -21,15 +21,12 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - ros_distro: [humble, iron] + ros_distro: [humble, jazzy] include: - ros_distro: 'humble' ubuntu_distro: 'jammy' - - ros_distro: 'iron' - ubuntu_distro: 'jammy' -# Disabled as there still missing dependencies on jazzy: -# - ros_distro: 'jazzy' -# ubuntu_distro: 'noble' + - 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 From 723ac7cdba43cefec497de5b562878c8973eab70 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 30 Nov 2024 19:58:41 -0800 Subject: [PATCH 063/126] CI: ros2 branch, only build for humble (all deps working) --- .github/workflows/ros2.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index ad83b0a8..96ce5778 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -3,7 +3,7 @@ name: ros2 on: push: - branches: [ ros2, humble-devel, jazzy-devel] + branches: [ ros2 ] pull_request: branches: [ ros2 ] @@ -21,12 +21,10 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - ros_distro: [humble, jazzy] + ros_distro: [humble] include: - ros_distro: 'humble' ubuntu_distro: 'jammy' - - 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 From d12f73af6a119f6b760ec9ba880a65e4926de637 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:46:41 -0800 Subject: [PATCH 064/126] bump 0.21.9 --- rtabmap_conversions/package.xml | 2 +- rtabmap_costmap_plugins/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_legacy/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index ed4abf29..1c6718d1 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.5 + 0.21.9 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index 1b89fd21..e81526de 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.5 + 0.21.9 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index b2806f86..2ba3f4bb 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.5 + 0.21.9 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 47d37539..1e178c53 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.5 + 0.21.9 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 8ee6de39..3a0fce7f 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.5 + 0.21.9 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index fb6e6d06..7273799d 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.5 + 0.21.9 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index a4236524..61dbdcae 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.5 + 0.21.9 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 22d65080..837afdc9 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.5 + 0.21.9 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 782efd83..8a0d0b23 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.5 + 0.21.9 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 85d968e2..caebced3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.5 + 0.21.9 RTAB-Map Stack From 7ca3a95899625b1fa48e5252be874a101e3eb4e0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:50:39 -0800 Subject: [PATCH 065/126] bump 0.21.9 remaining packages --- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index bf996032..a9030076 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.5 + 0.21.9 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 08e2fc06..2299ac09 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.5 + 0.21.9 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 83c43128..0f7c2625 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.5 + 0.21.9 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 13ef3227..33a58e81 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.5 + 0.21.9 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe From 6f81a8f70fac4eaa5e822edc32b82024ecf29189 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:55:09 -0800 Subject: [PATCH 066/126] missing 0.21.9 bump --- rtabmap_rviz_plugins/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index d604c7e9..48bab9cb 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.5 + 0.21.9 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe From e5667b280a516e1b610438d7985db67678016b33 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 14:36:14 -0800 Subject: [PATCH 067/126] Deskewing: add support for timestamp float64 field in nanoseconds (Livox) --- rtabmap_conversions/src/MsgConversion.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 5f59758e..9753bd88 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3058,6 +3058,25 @@ bool deskew_impl( } } + if(secFirst > 1.e18) + { + // convert nanoseconds to seconds + secFirst /= 1.e9; + secLast /= 1.e9; + } + else if(secFirst > 1.e15) + { + // convert microseconds to seconds + secFirst /= 1.e6; + secLast /= 1.e6; + } + else if(secFirst > 1.e12) + { + // convert milliseconds to seconds + secFirst /= 1.e3; + secLast /= 1.e3; + } + firstStamp = ros::Time(secFirst); lastStamp = ros::Time(secLast); } From 3bdbcdf3933d08abe258f12d7adf2b14ded1488c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 16:14:47 -0800 Subject: [PATCH 068/126] Follow-up of previous commit to convert all float64 timestamps correctly --- rtabmap_conversions/src/MsgConversion.cpp | 30 +++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 9753bd88..b41c7ae5 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3217,6 +3217,21 @@ bool deskew_impl( else if(timeDatatype == 8) //float64 { double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } @@ -3296,6 +3311,21 @@ bool deskew_impl( else if(timeDatatype == 8) { double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } From 42b0b49f5bcc182c6ccb604bdc4c076fb5bfaf16 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 17:45:11 -0800 Subject: [PATCH 069/126] Update README.md --- rtabmap_demos/README.md | 115 +++++++++++++++------------------------- 1 file changed, 43 insertions(+), 72 deletions(-) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index 8ab7a988..5e0b350d 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -1,101 +1,72 @@ # rtabmap_demos - -- [rtabmap_demos](#rtabmap-demos) - + [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) - + [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) - + [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) - + [Find-Object with SLAM](#find-object-with-slam) - + [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2--2d-lidar-and-rgb-d-slam) - + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) - + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) - + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2--2d-lidar-and-rgb-d-slam) - + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2--elevation-map-and-vslam) - + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--2d-lidar-and-rgb-d-slam) - + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-and-rgb-d-slam) - + [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-assembling-and-rgb-d-slam) - + [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) - + [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) ++ [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) ++ [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) ++ [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) ++ [Find-Object with SLAM](#find-object-with-slam) ++ [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) ++ [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) ++ [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) ++ [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) ++ [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-assembling-and-rgb-d-slam) ++ [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) ++ [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) ### Outdoor Stereo VSLAM -``` -ros2 launch rtabmap_demos 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) + ![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23) - ### Indoor 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true -``` +[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py) + ![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 -``` -ros2 launch rtabmap_demos 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) + ![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b) - ### Find-Object with SLAM -``` -ros2 launch rtabmap_demos 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) + ![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 -``` -ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py -``` +[turtlebot4_sim_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py) + ![Peek 2024-11-29 12-19](https://github.com/user-attachments/assets/5914e34c-19f1-4b7c-b4df-2e7084946888) - ### Turtlebot3 Nav2 and 2D LiDAR SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py -``` +[turtlebot3_sim_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py) + ![Peek 2024-11-29 12-23](https://github.com/user-attachments/assets/e3c31c5a-5c46-4370-ad17-38c795db7917) - ### Turtlebot3 Nav2 and RGB-D SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py -``` +[turtlebot3_sim_rgbd_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py) + ![Peek 2024-11-29 14-22](https://github.com/user-attachments/assets/5088be17-0875-42cc-b863-d14468c67f26) - ### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py -``` +[turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py) + ![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783) - ### Champ Quadruped Nav2, Elevation Map and VSLAM -``` -ros2 launch rtabmap_demos champ_sim_vslam.launch.py -``` +[champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py) + ![Peek 2024-11-29 15-00](https://github.com/user-attachments/assets/d1a27c78-27bc-4901-82a7-59b5d24e6454) - ### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py -``` +[husky_sim_scan2d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py) + ![Peek 2024-11-29 15-30](https://github.com/user-attachments/assets/c8f79b86-253e-4c8e-ac7a-c26584f43fa4) - ### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py -``` +[husky_sim_scan3d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py) + ![Peek 2024-11-29 15-36](https://github.com/user-attachments/assets/a4b6e6ae-38ed-44da-bbfb-d3c30a301f9c) - ### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py -``` +[husky_sim_scan3d_assemble_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py) + ![Peek 2024-11-29 16-16](https://github.com/user-attachments/assets/b2235bd2-33d2-4c44-b6e9-9923a524632b) - ### Isaac Sim Nav2 and Stereo SLAM -``` -ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py -``` -![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) +![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) ### Isaac Sim Nav2 and RGB-D VSLAM -``` -ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py stereo:=false vo:=rtabmap -``` -![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) \ No newline at end of file +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) stereo:=false vo:=rtabmap + +![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) From 9a4c585d5ef406d222e727645095a764163d8ca1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Dec 2024 17:26:46 -0800 Subject: [PATCH 070/126] Fixed https://github.com/introlab/rtabmap/issues/1399 --- .../include/rtabmap_odom/OdometryROS.h | 1 + rtabmap_odom/src/OdometryROS.cpp | 35 ++++++++++++++----- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index fe632fd4..c5672d36 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -148,6 +148,7 @@ private: USemaphore dataReady_; rtabmap::SensorData dataToProcess_; std_msgs::Header dataHeaderToProcess_; + bool bufferedDataToProcess_; bool paused_; int resetCountdown_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index a46d4fb2..a75ab336 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -436,13 +436,24 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); - UScopeMutex m(imuMutex_); - - imus_.insert(std::make_pair(stamp, imu)); - if(imus_.size() > 1000) { - NODELET_WARN("Dropping imu data!"); - imus_.erase(imus_.begin()); + UScopeMutex m(imuMutex_); + + imus_.insert(std::make_pair(stamp, imu)); + if(imus_.size() > 1000) + { + NODELET_WARN("Dropping imu data!"); + imus_.erase(imus_.begin()); + } + } + if(dataMutex_.lockTry() == 0) + { + if(bufferedDataToProcess_ && dataHeaderToProcess_.stamp.toSec() <= stamp) + { + bufferedDataToProcess_ = false; + dataReady_.release(); + } + dataMutex_.unlock(); } } } @@ -454,6 +465,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header { dataToProcess_ = data; dataHeaderToProcess_ = header; + bufferedDataToProcess_ = false; dataReady_.release(); dataMutex_.unlock(); } @@ -497,8 +509,15 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(bufferedDataToProcess_) { + NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + } + else { + NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; + } return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) From 01b8ab80ee122bca574269c97fc73994c8cdec59 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Dec 2024 17:47:36 -0800 Subject: [PATCH 071/126] refactored last commit --- rtabmap_odom/src/OdometryROS.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index a75ab336..0155b86e 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -463,6 +463,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); if(dataMutex_.lockTry() == 0) { + if(bufferedDataToProcess_) { + NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec()); + } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; @@ -509,15 +513,9 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - if(bufferedDataToProcess_) { - NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); - } - else { - NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); - bufferedDataToProcess_ = true; - } + NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) @@ -1128,6 +1126,7 @@ void OdometryROS::reset(const Transform & pose) imuProcessed_ = false; dataToProcess_ = SensorData(); dataHeaderToProcess_ = std_msgs::Header(); + bufferedDataToProcess_ = false; imuMutex_.lock(); imus_.clear(); imuMutex_.unlock(); From ef8ec4357d76fc83351acfaff0fc31c778214f46 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 13 Dec 2024 14:33:35 -0800 Subject: [PATCH 072/126] Fixed vo reset from guess (after being lost) not correctly updated if it is still lost after auto reset countdown --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 0155b86e..f79fca44 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -905,10 +905,9 @@ void OdometryROS::mainLoop() "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); } - else + else if(--resetCurrentCount_>0) { NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); - --resetCurrentCount_; } if(resetCurrentCount_ == 0 || tooOldPreviousData) @@ -936,6 +935,11 @@ void OdometryROS::mainLoop() odometry_->reset(tfPose); } } + // Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features). + // This will make sure we keep updating to latest guess pose. + if(resetCurrentCount_ == 0) { + ++resetCurrentCount_; + } } } From 856b2340b99809c19d86918e953c429253d9b201 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 3 Feb 2025 20:59:28 -0800 Subject: [PATCH 073/126] Added delete_db_on_start parameter to be usable with ComposableNode (#1058). --- rtabmap_slam/src/CoreWrapper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 73424801..3816cc41 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -346,7 +346,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } // declare parameters - this->declare_parameter("is_rtabmap_paused", paused_); + paused_ = this->declare_parameter("is_rtabmap_paused", paused_); if(paused_) { RCLCPP_WARN(get_logger(), "Node paused... don't forget to call service \"resume\" to start rtabmap."); @@ -388,6 +388,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : char ** argv = new char*[argList.size()]; bool deleteDbOnStart = false; + deleteDbOnStart = this->declare_parameter("delete_db_on_start", deleteDbOnStart); for(unsigned int i=0; i Date: Wed, 12 Feb 2025 19:25:32 -0800 Subject: [PATCH 074/126] bump 0.21.10 --- rtabmap_conversions/package.xml | 2 +- rtabmap_costmap_plugins/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_legacy/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- rtabmap_rviz_plugins/package.xml | 2 +- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 1c6718d1..e0454f93 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.9 + 0.21.10 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index e81526de..70f746dd 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.9 + 0.21.10 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 2ba3f4bb..560d8ad7 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.9 + 0.21.10 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 1e178c53..ddb8cc10 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.9 + 0.21.10 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 3a0fce7f..4f9914a1 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.9 + 0.21.10 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 7273799d..883e4bb2 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.9 + 0.21.10 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index 61dbdcae..3371b25a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.9 + 0.21.10 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 837afdc9..6b828b33 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.9 + 0.21.10 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 8a0d0b23..07cad0da 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.9 + 0.21.10 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index caebced3..0d0a21d3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.9 + 0.21.10 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 48bab9cb..7ff1e174 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.9 + 0.21.10 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index a9030076..4427bd1c 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.9 + 0.21.10 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 2299ac09..6bdc9e28 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.9 + 0.21.10 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 0f7c2625..2286a923 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.9 + 0.21.10 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 33a58e81..4bcba5fd 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.9 + 0.21.10 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe From e561bd190f5a6dbefa64e56ac4787d8dc7cedf84 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 16 Feb 2025 21:14:26 -0800 Subject: [PATCH 075/126] Updated clearpath lidar3d demos with workaround from https://github.com/gazebosim/gz-sim/issues/2743 --- .../launch/husky/husky_sim_scan3d_assemble_demo.launch.py | 3 +++ rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py index 9bfc6def..1c60eb8f 100644 --- a/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py @@ -8,6 +8,9 @@ # 320 # 240 # +# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743): +# - ogre2 +# + ogre # # Example with gazebo: # 1) Launch simulator (husky, nav2 and rtabmap): diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py index 5e8fab8c..d8807f99 100644 --- a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py @@ -8,6 +8,9 @@ # 320 # 240 # +# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743): +# - ogre2 +# + ogre # # Example with gazebo: # 1) Launch simulator (husky, nav2 and rtabmap): From 887695298a54ce01e53dfa9396ac958ff5161c5f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 16:31:16 -0800 Subject: [PATCH 076/126] Updated OdomInfo msg with upstream new stats --- rtabmap_conversions/src/MsgConversion.cpp | 9 +++++++++ rtabmap_msgs/msg/OdomInfo.msg | 2 ++ 2 files changed, 11 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index b41c7ae5..aacdcc4f 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1598,6 +1598,11 @@ std::map 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)); + + float32 localBundleAvgInlierDistance +int32 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)); @@ -1674,6 +1679,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::OdomInfo & msg, bool i info.localBundleOutliers = msg.localBundleOutliers; info.localBundleConstraints = msg.localBundleConstraints; info.localBundleTime = msg.localBundleTime; + info.localBundleAvgInlierDistance = msg.localBundleAvgInlierDistance; + info.localBundleMaxKeyFramesForInlier = msg.localBundleMaxKeyFramesForInlier; UASSERT(msg.localBundleModels.size() == msg.localBundleIds.size()); UASSERT(msg.localBundleModels.size() == msg.localBundlePoses.size()); for(size_t i=0; i >::const_iterator iter=info.localBundleModels.begin(); iter!=info.localBundleModels.end(); diff --git a/rtabmap_msgs/msg/OdomInfo.msg b/rtabmap_msgs/msg/OdomInfo.msg index 473b4314..094cf3e1 100644 --- a/rtabmap_msgs/msg/OdomInfo.msg +++ b/rtabmap_msgs/msg/OdomInfo.msg @@ -18,6 +18,8 @@ int32 localKeyFrames int32 localBundleOutliers int32 localBundleConstraints float32 localBundleTime +float32 localBundleAvgInlierDistance +int32 localBundleMaxKeyFramesForInlier bool keyFrameAdded float32 timeEstimation float32 timeParticleFiltering From a57b2971f2079d375f867bdb6927efefd75ddd38 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 16:41:24 -0800 Subject: [PATCH 077/126] fixed last commit --- rtabmap_conversions/src/MsgConversion.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index aacdcc4f..e8cb1ee0 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1600,9 +1600,6 @@ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & 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)); - - float32 localBundleAvgInlierDistance -int32 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)); From c4a596f8008428717ce5c45e48a5798e4e079235 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 15:50:39 -0800 Subject: [PATCH 078/126] Support any frame_id in initialpose #1273 --- rtabmap_slam/src/CoreWrapper.cpp | 37 +++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 08b56b4b..a26a58e3 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2526,14 +2526,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) { - Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose); - if(intialPose.isNull()) + Transform mapToPose = Transform::getIdentity(); + if(msg->header.frame_id.empty()) { - NODELET_ERROR("Pose received is null!"); - return; + NODELET_WARN("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, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); + if(mapToPose.isNull()) + { + NODELET_ERROR("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()) + { + NODELET_ERROR("initialpose received is null!"); + return; + } + if(mapToPose.isIdentity()) + { + NODELET_INFO("initialpose received: %s", initialPose.prettyPrint().c_str()); + rtabmap_.setInitialPose(initialPose); + } + else + { + NODELET_INFO("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( From 12e61a952e1ee977885009c006bd71dfb752857a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 15:53:54 -0800 Subject: [PATCH 079/126] bump required min rtabmap version --- rtabmap_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 166b0abb..b722d77d 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.5 REQUIRED) +find_package(RTABMap 0.21.11 REQUIRED) catkin_package( INCLUDE_DIRS include From 81d99d2c1c7877cca8dfc93de908ce8a1e7db137 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 17:43:22 -0800 Subject: [PATCH 080/126] Improved rgbdslam_datasets.launch.py to re-use vo features in back-end (#1277), also added ground truth recording. --- .../launch/rgbdslam_datasets.launch.py | 59 +++++++++++++------ 1 file changed, 42 insertions(+), 17 deletions(-) diff --git a/rtabmap_examples/launch/rgbdslam_datasets.launch.py b/rtabmap_examples/launch/rgbdslam_datasets.launch.py index 9aab2edd..92e8e030 100644 --- a/rtabmap_examples/launch/rgbdslam_datasets.launch.py +++ b/rtabmap_examples/launch/rgbdslam_datasets.launch.py @@ -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( From f77b3cdfc173edb92b2c4b39dcdf1707adc54bad Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 20:58:54 -0800 Subject: [PATCH 081/126] Updated readme --- README.md | 6 +----- docker/README.md | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 66ffb809..6eb481f8 100644 --- a/README.md +++ b/README.md @@ -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 Humble Build Status - - Iron - Build Status - Jazzy Build Status diff --git a/docker/README.md b/docker/README.md index 8af5659c..6714f614 100644 --- a/docker/README.md +++ b/docker/README.md @@ -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. From e04a2dfcae0c21cd880f8bfc49d64a862f274ddd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 21:02:50 -0800 Subject: [PATCH 082/126] CI: enabling jazzy build --- .github/workflows/ros2.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 96ce5778..0c7c9f87 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -21,10 +21,12 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - ros_distro: [humble] + ros_distro: [humble, jazzy] include: - ros_distro: 'humble' ubuntu_distro: 'jammy' + - 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 From 970d06307ff93ad454cc6e683a9511c2caf44e3d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Mar 2025 14:15:10 -0800 Subject: [PATCH 083/126] CI: Update docker-ros2.yml Related to #1285 --- .github/workflows/docker-ros2.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index f7069e80..dbc934c3 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -7,7 +7,7 @@ on: jobs: docker: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 strategy: matrix: @@ -36,24 +36,24 @@ jobs: 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 From 89cbdf7e435c08fc982ceab5cba7343040cddc22 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Mar 2025 14:59:25 -0800 Subject: [PATCH 084/126] CI: Update docker-ros2.yml Trying known working build on ubuntu 20.04: see #1285 --- .github/workflows/docker-ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index dbc934c3..3377de60 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -7,7 +7,7 @@ on: jobs: docker: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 strategy: matrix: From d9564b35b1c43ba04a7a14cc297fbd87bcddfaeb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 4 Mar 2025 18:39:44 -0800 Subject: [PATCH 085/126] CI: reverted to use latest ubuntu base image (docker ros2) --- .github/workflows/docker-ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 3377de60..17b7abc7 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -7,7 +7,7 @@ on: jobs: docker: - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: matrix: From 379b5ca47a126cb558318bbf8d2f5bbeecac67eb Mon Sep 17 00:00:00 2001 From: Ray Ferric <63957587+rayferric@users.noreply.github.com> Date: Sun, 16 Mar 2025 02:24:29 +0100 Subject: [PATCH 086/126] rgb_transport and depth_transport params (#1282) --- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 451973de..1da53fc4 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -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) { From 3b5766361dadba5ef30cf6b8a5bcfeb0522af3c0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 17 Mar 2025 16:05:07 -0700 Subject: [PATCH 087/126] Update docker-ros2.yml --- .github/workflows/docker-ros2.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 17b7abc7..a7ddc927 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -10,6 +10,7 @@ jobs: runs-on: ubuntu-latest strategy: + fail-fast: false matrix: docker_tag: [humble, humble-latest, jazzy, jazzy-latest] include: From 70ddb3fc839394a5b440fee56f1877c508d6b9ea Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 22 Mar 2025 21:09:28 -0700 Subject: [PATCH 088/126] Fixed occupancy grid reset in localization mode when we update parameters live, losing optimized map if we had one. --- rtabmap_slam/src/CoreWrapper.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index d0d57ebc..80a16b7c 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -922,7 +922,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_); + } } }; @@ -2971,7 +2974,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( From 8c1513bc3bfc16e15bfde9573895e20edb4a617f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 22 Mar 2025 21:14:47 -0700 Subject: [PATCH 089/126] cherry pick https://github.com/introlab/rtabmap_ros/commit/70ddb3fc839394a5b440fee56f1877c508d6b9ea --- rtabmap_slam/src/CoreWrapper.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index a26a58e3..ec9cc4a1 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2815,7 +2815,10 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp NODELET_INFO("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_); + } return true; } From 3ed09452f63a701bf59b68d54c4b49a0b31f1bd5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Mar 2025 09:44:31 -0700 Subject: [PATCH 090/126] Update docker-ros2.yml --- .github/workflows/docker-ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index a7ddc927..94e0648c 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -61,6 +61,6 @@ jobs: 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 From e1e2ce5eced1c05b478454b5f2de22484f04873a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Mar 2025 11:58:39 -0700 Subject: [PATCH 091/126] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bf16e219..8a22a85b 100644 --- a/README.md +++ b/README.md @@ -125,7 +125,7 @@ This section shows how to install RTAB-Map ros-pkg on **ROS Noetic** (Catkin bui ```bash cd ~/catkin_ws git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros - catkin_make -j4 + catkin_make -DCMAKE_BUILD_TYPE=Release -j4 ``` * Use `catkin_make -j1` if compilation requires more RAM than you have (e.g., some files require up to ~2 GB to build depending on gcc version). * Options: @@ -158,7 +158,7 @@ roscd rtabmap_ros git pull origin master roscd cd .. -catkin_make -j1 --pkg rtabmap_ros +catkin_make -j1 -DCMAKE_BUILD_TYPE=Release --pkg rtabmap_ros ``` From b0916ad14c8826f4bdba87cd17db0065bc6d6b84 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Mar 2025 12:36:43 -0700 Subject: [PATCH 092/126] imu_to_tf: fixed fixed_frame to base_frame orientation when IMU's frame z axis is not up accordingly to base frame --- rtabmap_util/src/nodelets/imu_to_tf.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 49b22592..42d39713 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -87,8 +87,10 @@ private: } tf::StampedTransform tmp; - tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp); - tf::Transform t = tmp.inverse()*st*tmp; + tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp); + tf::Quaternion q; + q.setRPY(0.0,0.0,tf::getYaw(tmp.getRotation())); + tf::Transform t = tf::Transform(q)*st*tmp.inverse(); // base_frame orientation st.setRotation(t.getRotation()); st.child_frame_id_ = baseFrameId_; } From 001df6a17dd1706de0589d5f8beba9678af8dd47 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Mar 2025 14:52:04 -0700 Subject: [PATCH 093/126] GUI/Download map: Fixed parameters not read when downloading map --- rtabmap_viz/CMakeLists.txt | 9 ++++++++- rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h | 3 ++- rtabmap_viz/src/PreferencesDialogROS.cpp | 8 ++++++-- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 5e02ca65..5c196b6c 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -33,11 +33,18 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -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) IF(Qt5_FOUND) QT5_USE_MODULES(rtabmap_viz Widgets Core Gui) ENDIF(Qt5_FOUND) target_link_libraries(rtabmap_viz ${catkin_LIBRARIES}) +SET_TARGET_PROPERTIES( + rtabmap_viz + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) ############# diff --git a/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h b/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h index 7c2b1602..6497d9f8 100644 --- a/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h +++ b/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h @@ -35,6 +35,7 @@ using namespace rtabmap; class PreferencesDialogROS : public PreferencesDialog { + Q_OBJECT public: PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName); virtual ~PreferencesDialogROS(); @@ -43,7 +44,7 @@ public: virtual QString getTmpIniFilePath() const; bool hasAllParameters(); -public slots: +public Q_SLOTS: void readRtabmapNodeParameters(); protected: diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index 83a5931f..8fcfabd1 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -89,7 +89,9 @@ bool PreferencesDialogROS::hasAllParameters() rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); for(rtabmap::ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i) { - if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && !nh.hasParam(i->first)) + if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && + !nh.hasParam(i->first) && + i->first.find("Odom") != 0) { return false; } @@ -101,7 +103,9 @@ bool PreferencesDialogROS::hasAllParameters(const ros::NodeHandle & nh, const rt { for(rtabmap::ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i) { - if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && !nh.hasParam(i->first)) + if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && + !nh.hasParam(i->first) && + i->first.find("Odom") != 0) { return false; } From a2f2971094dec3644e55e54b5fe2eed3f2eee3eb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Mar 2025 21:29:46 -0700 Subject: [PATCH 094/126] Updated lidar3d examples (and new 2x lidars example) --- rtabmap_examples/launch/lidar3d.launch.py | 25 +- .../launch/lidar3d_assemble.launch.py | 69 +++-- .../launch/lidar3d_assemble_x2.launch.py | 291 ++++++++++++++++++ .../src/nodelets/point_cloud_aggregator.cpp | 21 +- .../src/nodelets/point_cloud_assembler.cpp | 10 +- 5 files changed, 379 insertions(+), 37 deletions(-) create mode 100644 rtabmap_examples/launch/lidar3d_assemble_x2.launch.py diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index daec929f..b752c3e3 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -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. # @@ -46,6 +46,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' + fixed_frame_from_imu = False fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) if not fixed_frame_id and imu_used: @@ -78,10 +81,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs): } icp_odometry_parameters = { - 'expected_update_rate': 15.0, + 'expected_update_rate': LaunchConfiguration('expected_update_rate'), 'deskewing': not fixed_frame_id, # 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), @@ -106,7 +110,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'Mem/NotLinkedNodesKept': 'false', 'Mem/STMSize': '30', 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': '0.2' + 'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap') } arguments = [] @@ -162,7 +166,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 +211,21 @@ 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( + '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', diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 118cbcfd..213f15a6 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -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,11 +28,16 @@ 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') @@ -51,6 +56,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 +82,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 +97,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,7 +111,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'Mem/NotLinkedNodesKept': 'false', 'Mem/STMSize': '30', 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': '0.2' + 'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap') } remappings = [('imu', imu_topic), @@ -116,6 +125,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 +138,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,8 +159,8 @@ 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,}], + 'topic_queue_size': 40, + 'sync_queue_size': 40,}], remappings=remappings + [('scan_cloud', 'assembled_cloud')], arguments=arguments), @@ -159,9 +168,17 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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 +207,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', @@ -212,10 +233,22 @@ def generate_launch_description(): '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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py new file mode 100644 index 00000000..ff44b19e --- /dev/null +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -0,0 +1,291 @@ +# 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_image_used = rgbd_image_topic.perform(context) != '' + + 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': LaunchConfiguration('min_loop_closure_overlap') + } + + remappings = [('imu', imu_topic), + ('odom', 'icp_odom')] + if rgbd_image_used: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_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, + 'topic_queue_size': 40, + 'sync_queue_size': 40,}], + remappings=remappings + [('scan_cloud', 'assembled_cloud')], + 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( + '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( + '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), + ]) + + diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 27401b55..4891aee7 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -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() diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 688a634a..2edf5509 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -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([&](){ From e661abb884018b05a0951c3c2d20d8c53149fc20 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Mar 2025 14:15:09 -0700 Subject: [PATCH 095/126] GPS and global pose async topics buffered to get more accuratly the closest value to current update stamp in case there is significant delay. --- .../rtabmap_conversions/MsgConversion.h | 36 ++++++ rtabmap_conversions/src/MsgConversion.cpp | 2 +- .../include/rtabmap_slam/CoreWrapper.h | 9 +- rtabmap_slam/src/CoreWrapper.cpp | 107 +++++++++++++----- 4 files changed, 125 insertions(+), 29 deletions(-) diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 904731c2..3d9f5daa 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -323,6 +323,42 @@ inline int sizeOfPointField(int datatype) } return -1; } + +template +typename std::map::const_iterator getClosestIterator( + const std::map & buffer, + const K & key) +{ + UASSERT(!buffer.empty()); + typename std::map::const_iterator iterB = buffer.lower_bound(key); + typename std::map::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_ */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index dc94f69d..be80ce78 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3386,7 +3386,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; } diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index e0146aac..a0eb6e73 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -405,10 +405,15 @@ private: cv::Mat userData_; UMutex userDataMutex_; + rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr globalPoseAsyncSub_; - geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; + std::map globalPoses_; + UMutex globalPoseMutex_; + + rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; - rtabmap::GPS gps_; + std::map gps_; + UMutex gpsMutex_; rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 80a16b7c..95944bf9 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -145,7 +145,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); @@ -844,12 +843,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 +863,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("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("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions); - gpsFixAsyncSub_ = this->create_subscription("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("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions); + gpsFixAsyncSub_ = this->create_subscription("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("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS @@ -2059,18 +2064,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(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 @@ -2078,7 +2105,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()) @@ -2091,17 +2118,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::const_iterator iter = rtabmap_conversions::getClosestIterator(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(); @@ -2520,7 +2562,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()); + } } } @@ -2537,13 +2584,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()); + } } } @@ -3003,8 +3058,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(); @@ -3105,8 +3160,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(); @@ -3241,8 +3296,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(); From f5a3e487056e399b834971ef6a64126843666f7f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Mar 2025 14:17:34 -0700 Subject: [PATCH 096/126] lidar3d examples: added gps_topic argument --- rtabmap_examples/launch/lidar3d_assemble.launch.py | 6 +++++- rtabmap_examples/launch/lidar3d_assemble_x2.launch.py | 6 +++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 213f15a6..3efa234a 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -161,7 +161,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): {'subscribe_rgbd': rgbd_image_used, 'topic_queue_size': 40, 'sync_queue_size': 40,}], - remappings=remappings + [('scan_cloud', 'assembled_cloud')], + remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))], arguments=arguments), # Just for visualization @@ -225,6 +225,10 @@ 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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index ff44b19e..4eac3846 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -189,7 +189,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): {'subscribe_rgbd': rgbd_image_used, 'topic_queue_size': 40, 'sync_queue_size': 40,}], - remappings=remappings + [('scan_cloud', 'assembled_cloud')], + remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))], arguments=arguments), # Just for visualization @@ -257,6 +257,10 @@ 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.'), From f122dbace59dab2179098736a203ec7620f59cba Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Apr 2025 15:22:34 -0700 Subject: [PATCH 097/126] bump 0.21.13 --- rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_conversions/package.xml | 2 +- rtabmap_costmap_plugins/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_legacy/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- rtabmap_rviz_plugins/package.xml | 2 +- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 16 files changed, 16 insertions(+), 16 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index b722d77d..33c8a0e0 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.11 REQUIRED) +find_package(RTABMap 0.21.13 REQUIRED) catkin_package( INCLUDE_DIRS include diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index e0454f93..ebd35cff 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.10 + 0.21.13 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index 70f746dd..f180c6d9 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.10 + 0.21.13 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 560d8ad7..c2d2e8dd 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.10 + 0.21.13 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index ddb8cc10..513e175a 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.10 + 0.21.13 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 4f9914a1..a6fb2231 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.10 + 0.21.13 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 883e4bb2..daa0099c 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.10 + 0.21.13 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index 3371b25a..c0bfcefe 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.10 + 0.21.13 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 6b828b33..f18ca5cc 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.10 + 0.21.13 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 07cad0da..9cbc0291 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.10 + 0.21.13 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 0d0a21d3..4334a45e 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.10 + 0.21.13 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 7ff1e174..40df0ed8 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.10 + 0.21.13 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 4427bd1c..c34d3dc5 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.10 + 0.21.13 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 6bdc9e28..7cff3a50 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.10 + 0.21.13 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 2286a923..d33f68dd 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.10 + 0.21.13 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 4bcba5fd..15a19976 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.10 + 0.21.13 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe From b7ce0b19c2a7cfa89af768fbf120a702190619a9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 29 Apr 2025 14:30:44 -0700 Subject: [PATCH 098/126] Update ros1.yml --- .github/workflows/ros1.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index 39f25e63..c0d6c707 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -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. From 4e6924979d58fc03878dec2ed900ddeec6e3eaa8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 1 May 2025 19:14:28 -0700 Subject: [PATCH 099/126] lidar3d examples: fixed min_loop_closure_overlap parameter not converted to string. --- rtabmap_examples/launch/lidar3d.launch.py | 12 ++++++++---- rtabmap_examples/launch/lidar3d_assemble.launch.py | 2 +- .../launch/lidar3d_assemble_x2.launch.py | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index b752c3e3..89fbe205 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -46,6 +46,9 @@ 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' @@ -55,7 +58,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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: @@ -82,7 +85,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): icp_odometry_parameters = { 'expected_update_rate': LaunchConfiguration('expected_update_rate'), - 'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below + '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, @@ -101,6 +104,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'subscribe_rgb': False, 'subscribe_odom_info': True, 'subscribe_scan_cloud': True, + 'map_frame_id': 'new_map', # RTAB-Map's internal parameters are strings: 'RGBD/ProximityMaxGraphDepth': '0', 'RGBD/ProximityPathMaxNeighbors': '1', @@ -110,7 +114,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'Mem/NotLinkedNodesKept': 'false', 'Mem/STMSize': '30', 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap') + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) } arguments = [] @@ -158,7 +162,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( diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 3efa234a..030a085c 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -111,7 +111,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'Mem/NotLinkedNodesKept': 'false', 'Mem/STMSize': '30', 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap') + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) } remappings = [('imu', imu_topic), diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index 4eac3846..8aceaab0 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -115,7 +115,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'Mem/NotLinkedNodesKept': 'false', 'Mem/STMSize': '30', 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap') + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) } remappings = [('imu', imu_topic), From bea1eda7135fc166750e16ef7cd3825e52ee2f42 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 1 May 2025 19:52:44 -0700 Subject: [PATCH 100/126] README: added tip on CycloneDDS to disable multicast --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 6eb481f8..de5a2176 100644 --- a/README.md +++ b/README.md @@ -68,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="0.0.0.0" ``` # Installation From e9fa89110c8e8d09afdebe7349f4f7ead12159d0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 6 May 2025 16:14:58 -0700 Subject: [PATCH 101/126] load_database srv: fixed Odom parameters missing warnings, fixed warning when existing parameters are the the same than loaded ones from new database. --- rtabmap_slam/src/CoreWrapper.cpp | 38 +++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index ec9cc4a1..6a3ab207 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -359,7 +359,7 @@ void CoreWrapper::onInit() double vDouble; if(pnh.getParam(iter->first, vStr)) { - NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), vStr.c_str()); if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) { @@ -373,17 +373,17 @@ void CoreWrapper::onInit() } else if(pnh.getParam(iter->first, vBool)) { - NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uBool2Str(vBool).c_str()); uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool))); } else if(pnh.getParam(iter->first, vDouble)) { - NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uNumber2Str(vDouble).c_str()); uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble))); } else if(pnh.getParam(iter->first, vInt)) { - NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uNumber2Str(vInt).c_str()); uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt))); } } @@ -2942,14 +2942,16 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req, // 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) @@ -2959,15 +2961,31 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req, // 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) { - NODELET_WARN("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()) + { + NODELET_WARN("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) + { + NODELET_WARN("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()); } } From 6c6c41a7e2574597683b8ab7e5e99eacc9cf0be0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 May 2025 14:15:40 -0700 Subject: [PATCH 102/126] Update README.md Added video links for some demos --- rtabmap_demos/README.md | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index 5e0b350d..b1eb14ce 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -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) From 43d360ebb3e7b67c57c05a8c3f55f292f3d00f52 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 26 May 2025 16:00:16 -0700 Subject: [PATCH 103/126] Fixed build with latest rtabmap version --- rtabmap_conversions/src/MsgConversion.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index be80ce78..3c97a2c4 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1652,9 +1652,8 @@ std::map 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); From 5676b9a69e95d73fc4fd056d7ec772a3e1fe5477 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 26 May 2025 16:16:36 -0700 Subject: [PATCH 104/126] Fixed build against latest rtabmap version --- rtabmap_conversions/src/MsgConversion.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index e8cb1ee0..a61544dd 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1630,9 +1630,8 @@ std::map 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); From b6584b0a100516c783fb9c3cae66f8b8e36882c6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Jun 2025 16:57:15 -0700 Subject: [PATCH 105/126] Fixed #1320 --- rtabmap_slam/src/CoreWrapper.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 95944bf9..2c39a9f6 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -720,7 +720,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; + } } }); } From aa1e17b1c15645f96be8c33dfc320cd574873066 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Jun 2025 13:31:50 -0700 Subject: [PATCH 106/126] Updated lidar3d examples with new option "rgbd_images_topic" to input multi-camera data. --- rtabmap_examples/launch/lidar3d.launch.py | 18 +++++++++++++++--- .../launch/lidar3d_assemble.launch.py | 14 ++++++++++++-- .../launch/lidar3d_assemble_x2.launch.py | 14 ++++++++++++-- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index 89fbe205..6ddee09a 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -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)) @@ -105,6 +107,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): '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', @@ -130,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( @@ -140,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), @@ -215,6 +223,10 @@ 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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 030a085c..9c217640 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -42,7 +42,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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) @@ -117,7 +119,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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: @@ -159,6 +164,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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'))], @@ -233,6 +239,10 @@ 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( '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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index 8aceaab0..23da663f 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -42,7 +42,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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 lidar1_topic = LaunchConfiguration('lidar1_topic') lidar1_topic_value = lidar1_topic.perform(context) @@ -121,7 +123,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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: @@ -187,6 +192,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 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'))], @@ -265,6 +271,10 @@ 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( '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.'), From 2d270e9c3cb5d39bbcab789f7e9f289ba55ab748 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Jun 2025 17:06:02 -0700 Subject: [PATCH 107/126] husky lidar3d demo: added option to disable camera --- .../launch/husky/husky_sim_scan3d_demo.launch.py | 3 +++ rtabmap_demos/launch/husky/husky_slam3d.launch.py | 12 ++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py index d8807f99..ed6c896a 100644 --- a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py @@ -43,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(): @@ -86,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')) ] ) diff --git a/rtabmap_demos/launch/husky/husky_slam3d.launch.py b/rtabmap_demos/launch/husky/husky_slam3d.launch.py index 2b36ddad..bbadcb25 100644 --- a/rtabmap_demos/launch/husky/husky_slam3d.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam3d.launch.py @@ -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}], From 50fe38ef6c9b311c6f54cb249c20afd004fc90ff Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 15:01:07 -0700 Subject: [PATCH 108/126] Adding ROS2 Kilted to CI (#1322) * Adding ROS2 Kilted to CI * updated ros2 ci workflow * updated version of action-ros-ci * Updated dev containers, fixed kilted build * Added git autocompletion --- .devcontainer/humble/Dockerfile | 17 +++++ .devcontainer/humble/devcontainer.json | 27 +++++-- .devcontainer/jazzy/Dockerfile | 20 +++++ .devcontainer/jazzy/devcontainer.json | 27 +++++-- .devcontainer/kilted/Dockerfile | 20 +++++ .devcontainer/kilted/devcontainer.json | 27 +++++++ .github/workflows/docker-ros2.yml | 7 +- .github/workflows/ros2.yml | 18 +---- docker/jazzy/Dockerfile | 1 - docker/jazzy/latest/Dockerfile | 2 +- docker/kilted/Dockerfile | 6 ++ docker/kilted/latest/Dockerfile | 19 +++++ rtabmap_slam/CMakeLists.txt | 4 + rtabmap_slam/src/CoreWrapper.cpp | 75 ++++++++++--------- .../rtabmap_sync/CommonDataSubscriber.h | 8 +- rtabmap_util/CMakeLists.txt | 4 + rtabmap_util/src/nodelets/imu_to_tf.cpp | 4 +- rtabmap_util/src/nodelets/map_assembler.cpp | 8 +- 18 files changed, 223 insertions(+), 71 deletions(-) create mode 100644 .devcontainer/humble/Dockerfile create mode 100644 .devcontainer/jazzy/Dockerfile create mode 100644 .devcontainer/kilted/Dockerfile create mode 100644 .devcontainer/kilted/devcontainer.json create mode 100644 docker/kilted/Dockerfile create mode 100644 docker/kilted/latest/Dockerfile diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 00000000..62978bb7 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -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 diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index 66de040e..a1666586 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -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"] } diff --git a/.devcontainer/jazzy/Dockerfile b/.devcontainer/jazzy/Dockerfile new file mode 100644 index 00000000..13b36c5a --- /dev/null +++ b/.devcontainer/jazzy/Dockerfile @@ -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 diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index fcdec049..5e768e9a 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -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"] } diff --git a/.devcontainer/kilted/Dockerfile b/.devcontainer/kilted/Dockerfile new file mode 100644 index 00000000..9aee0a83 --- /dev/null +++ b/.devcontainer/kilted/Dockerfile @@ -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 diff --git a/.devcontainer/kilted/devcontainer.json b/.devcontainer/kilted/devcontainer.json new file mode 100644 index 00000000..6176ee77 --- /dev/null +++ b/.devcontainer/kilted/devcontainer.json @@ -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"] +} diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 94e0648c..72a91300 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -12,7 +12,7 @@ jobs: 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' @@ -33,6 +33,11 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 + - docker_tag: kilted-latest + docker_path: 'kilted/latest' + docker_platforms: | + linux/amd64 + linux/arm64 steps: - diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 0c7c9f87..a7a26757 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -8,28 +8,18 @@ on: branches: [ ros2 ] 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: [humble, jazzy] - include: - - ros_distro: 'humble' - ubuntu_distro: 'jammy' - - ros_distro: 'jazzy' - ubuntu_distro: 'noble' + ros_distro: [humble, jazzy, kilted] 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/setup-ros@v0.7 @@ -38,7 +28,7 @@ jobs: - run: | echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\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 }} diff --git a/docker/jazzy/Dockerfile b/docker/jazzy/Dockerfile index 2de694ec..b845aa95 100644 --- a/docker/jazzy/Dockerfile +++ b/docker/jazzy/Dockerfile @@ -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 \ diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index a6c112e2..bf26d4d5 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -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 && \ diff --git a/docker/kilted/Dockerfile b/docker/kilted/Dockerfile new file mode 100644 index 00000000..6c0ba3b3 --- /dev/null +++ b/docker/kilted/Dockerfile @@ -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/ diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile new file mode 100644 index 00000000..a6c112e2 --- /dev/null +++ b/docker/kilted/latest/Dockerfile @@ -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 diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index ae9c6a84..d843f4f6 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -57,6 +57,10 @@ SET(Libraries rtabmap_sync ) +if("$ENV{ROS_DISTRO}" STRLESS "jazzy") + add_definitions(-DPRE_ROS_JAZZY) +endif() + ########### ## Build ## ########### diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 2c39a9f6..531dccbe 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -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 { @@ -657,45 +664,45 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : // setup services const std::string servicePrefix = get_name() + std::string("/"); - updateSrv_ = this->create_service(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + triggerNewMapSrv_ = this->create_service(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(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + detectMoreLoopClosuresSrv_ = this->create_service(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(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(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(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(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(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(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(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(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getProbMapSrv_ = this->create_service(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(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getPlanNodesSrv_ = this->create_service(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(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getNodesInRadiusSrv_ = this->create_service(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(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(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(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + octomapFullSrv_ = this->create_service(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(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(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(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(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(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogErrorSrv_ = this->create_service(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); @@ -885,8 +892,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : #endif imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1), imuSubOptions); republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 1, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1), subOptions); + parametersClient_ = std::make_shared(this, std::string(), rclcpp::ParametersQoS(), processingCallbackGroup_); - parametersClient_ = std::make_shared(this, std::string(), rmw_qos_profile_parameters, processingCallbackGroup_); auto on_parameter_event_callback = [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void { diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 20e5f2b4..1f3f63b2 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -29,10 +29,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 7ae96252..525d239e 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -56,6 +56,10 @@ SET(Libraries rtabmap_conversions ) +if("$ENV{ROS_DISTRO}" STRLESS "jazzy") + add_definitions(-DPRE_ROS_JAZZY) +endif() + ########### ## Build ## ########### diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index dc892f5f..636ba256 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -64,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; @@ -97,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_; diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp index 9874ab14..f966533c 100644 --- a/rtabmap_util/src/nodelets/map_assembler.cpp +++ b/rtabmap_util/src/nodelets/map_assembler.cpp @@ -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(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer + client_ = this->create_client(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_); } From 2f0ff060560baa396506e55b52129042d3d4bb82 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 22:30:06 +0000 Subject: [PATCH 109/126] Updated dev container --- .devcontainer/Dockerfile | 17 +++++++++++++++++ .devcontainer/devcontainer.json | 27 +++++++++++++++++++++------ 2 files changed, 38 insertions(+), 6 deletions(-) create mode 100644 .devcontainer/Dockerfile diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 00000000..3eec0a03 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -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 diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 13ff5c22..4990bced 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,12 +1,27 @@ { - "image": "introlab3it/rtabmap_ros:noetic-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/noetic/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" }, - "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_ros,type=bind", - "workspaceFolder": "/catkin_ws", - "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'", - "runArgs": ["--privileged"] + "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"] } From ba45ccd7a54e127f6364513e91f2403268eee7dd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 15:44:17 -0700 Subject: [PATCH 110/126] removed armv7 docker build --- .github/workflows/docker.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index ec6913e1..ab73b32e 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -32,7 +32,6 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 - linux/arm/v7 steps: - From 945101697a6435c83ee64f28a021a62aca5a12d8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 16:01:18 -0700 Subject: [PATCH 111/126] bump version 0.22.0 --- rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_conversions/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- rtabmap_rviz_plugins/package.xml | 2 +- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 1b07c203..f51f841b 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(RTABMap 0.21.13 REQUIRED) +find_package(RTABMap 0.22.0 REQUIRED) include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 911b04db..3a9484cf 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -2,7 +2,7 @@ rtabmap_conversions - 0.21.13 + 0.22.0 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 2e31d6b3..d65cbfc7 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -2,7 +2,7 @@ rtabmap_demos - 0.21.13 + 0.22.0 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index d83deda1..e851d2e3 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -2,7 +2,7 @@ rtabmap_examples - 0.21.13 + 0.22.0 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 43c89c6a..f674e8f9 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -2,7 +2,7 @@ rtabmap_launch - 0.21.13 + 0.22.0 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index e4ac814e..ca45d59a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -2,7 +2,7 @@ rtabmap_msgs - 0.21.13 + 0.22.0 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 274b1acf..ffa9cd90 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -2,7 +2,7 @@ rtabmap_odom - 0.21.13 + 0.22.0 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index ba2a97bb..d56a425c 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -2,7 +2,7 @@ rtabmap_python - 0.21.13 + 0.22.0 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 916c1347..2da81cf3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -2,7 +2,7 @@ rtabmap_ros - 0.21.13 + 0.22.0 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 54dd8952..32b779c4 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -2,7 +2,7 @@ rtabmap_rviz_plugins - 0.21.13 + 0.22.0 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index f332f664..ed6d9271 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -2,7 +2,7 @@ rtabmap_slam - 0.21.13 + 0.22.0 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index c3edcfde..8eee2e6a 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -2,7 +2,7 @@ rtabmap_sync - 0.21.13 + 0.22.0 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 326b2473..14a75a52 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -2,7 +2,7 @@ rtabmap_util - 0.21.13 + 0.22.0 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 14183c6e..de89ccd5 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -2,7 +2,7 @@ rtabmap_viz - 0.21.13 + 0.22.0 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe From fa342bd85350a887454d8105b9a29eae8e2c16c2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 16:15:37 -0700 Subject: [PATCH 112/126] removed ros1 workflow --- .github/workflows/ros1.yml | 67 -------------------------------------- 1 file changed, 67 deletions(-) delete mode 100644 .github/workflows/ros1.yml diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml deleted file mode 100644 index c0d6c707..00000000 --- a/.github/workflows/ros1.yml +++ /dev/null @@ -1,67 +0,0 @@ -name: ros1 - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) - BUILD_TYPE: Release - -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. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-20.04] - include: - - os: ubuntu-20.04 - ros_distro: 'noetic' - - - steps: - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ matrix.ros_distro }} - - - name: Install dependencies - run: | - sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools - sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap - sudo pip3 uninstall empy --yes - - - name: Setup catkin workspace - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - mkdir -p ${{github.workspace}}/catkin_ws/src - cd ${{github.workspace}}/catkin_ws/src - cd .. - catkin config --init --cmake-args -DSETUPTOOLS_DEB_LAYOUT=OFF -DCMAKE_C_FLAGS="-Wformat -Werror=format-security" -DCMAKE_CXX_FLAGS="-Wformat -Werror=format-security" - - - uses: actions/checkout@v2 - with: - repository: 'introlab/rtabmap' - path: 'catkin_ws/src/rtabmap' - - - uses: actions/checkout@v2 - with: - path: 'catkin_ws/src/rtabmap_ros' - - - name: caktkin build - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - cd ${{github.workspace}}/catkin_ws - catkin build -p 1 -i --verbose From a74c3acda528425e63b93b042b1509fe11f7082b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 19:54:30 -0700 Subject: [PATCH 113/126] Added apriltag_msgs as package dependency --- rtabmap_slam/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index ed6d9271..053e5840 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -24,6 +24,7 @@ tf2 tf2_ros visualization_msgs + apriltag_msgs rtabmap_msgs rtabmap_util From 6cbee86626c27d6a8a3534995c363cf853ef7a5e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Jun 2025 20:14:21 -0700 Subject: [PATCH 114/126] CI: humble: force upgrade packages --- docker/humble/latest/Dockerfile | 2 +- docker/humble/latest/hooks/build | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) delete mode 100644 docker/humble/latest/hooks/build diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index b7d01701..74fa564f 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -12,8 +12,8 @@ RUN source /ros_entrypoint.sh && \ rosdep init && \ rosdep update && \ apt-get update && \ + apt-get upgrade && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ - apt remove ros-$ROS_DISTRO-rtabmap* -y && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ diff --git a/docker/humble/latest/hooks/build b/docker/humble/latest/hooks/build deleted file mode 100644 index bffa442b..00000000 --- a/docker/humble/latest/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:humble-latest . From 7503064fe155c9095b72fa0be5546fedb9d8ccd1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Jun 2025 20:17:00 -0700 Subject: [PATCH 115/126] CI: humble revert uninstall rtabmap binaries --- docker/humble/latest/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index 74fa564f..d42b15d4 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -14,6 +14,7 @@ RUN source /ros_entrypoint.sh && \ apt-get update && \ apt-get upgrade && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ + apt remove ros-$ROS_DISTRO-rtabmap -y && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ From 62233708fb17929bf958e729bc0af6e72302b571 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Jun 2025 20:19:33 -0700 Subject: [PATCH 116/126] CI: humble: missing -y --- docker/humble/latest/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index d42b15d4..9018be59 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -12,7 +12,7 @@ RUN source /ros_entrypoint.sh && \ rosdep init && \ rosdep update && \ apt-get update && \ - apt-get upgrade && \ + apt-get upgrade -y && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ apt remove ros-$ROS_DISTRO-rtabmap -y && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ From a7d128f48e4d45e492b25669534d3ac68d9e163b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Jun 2025 20:33:24 -0700 Subject: [PATCH 117/126] CI:humble: try rebuilding also rtabmap --- docker/humble/latest/Dockerfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index 9018be59..e4d6d3b2 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -1,4 +1,4 @@ -FROM introlab3it/rtabmap:22.04 +FROM introlab3it/rtabmap:jammy-deps RUN source /ros_entrypoint.sh && \ mkdir -p ros2_ws/src && \ @@ -13,8 +13,8 @@ RUN source /ros_entrypoint.sh && \ rosdep update && \ apt-get update && \ apt-get upgrade -y && \ + git clone https://github.com/introlab/rtabmap.git src/rtabmap && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ - apt remove ros-$ROS_DISTRO-rtabmap -y && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ From 4291d5a18d2f64cf1cf87e0e552b37489e8871bc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 10 Jun 2025 08:39:16 -0700 Subject: [PATCH 118/126] CI: put back only rtabmap_ros to build --- docker/humble/latest/Dockerfile | 4 +--- docker/jazzy/latest/Dockerfile | 2 +- docker/kilted/latest/Dockerfile | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index e4d6d3b2..67fe3c31 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -1,4 +1,4 @@ -FROM introlab3it/rtabmap:jammy-deps +FROM introlab3it/rtabmap:jammy RUN source /ros_entrypoint.sh && \ mkdir -p ros2_ws/src && \ @@ -12,8 +12,6 @@ RUN source /ros_entrypoint.sh && \ rosdep init && \ rosdep update && \ apt-get update && \ - apt-get upgrade -y && \ - git clone https://github.com/introlab/rtabmap.git src/rtabmap && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index bf26d4d5..3b60aaac 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -1,4 +1,4 @@ -FROM introlab3it/rtabmap:24.04 +FROM introlab3it/rtabmap:noble RUN source /ros_entrypoint.sh && \ mkdir -p ros2_ws/src && \ diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index a6c112e2..2a20a5ea 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -1,4 +1,4 @@ -FROM introlab3it/rtabmap:24.04 +FROM introlab3it/rtabmap:noble-kilted RUN source /ros_entrypoint.sh && \ mkdir -p ros2_ws/src && \ From 600732f44bce9637b308a0609e07507663176bd4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 10 Jun 2025 10:46:34 -0700 Subject: [PATCH 119/126] CI: skip explicitly keys for ros2 workflow --- .github/workflows/ros2.yml | 8 ++++++++ docker/humble/latest/Dockerfile | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index a7a26757..790f78c4 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -17,6 +17,13 @@ jobs: strategy: matrix: ros_distro: [humble, jazzy, kilted] + include: + - ros_distro: humble + skip_keys: '' + - ros_distro: jazzy + skip_keys: '' + - ros_distro: kilted + skip_keys: 'nav2_bringup nav2_msgs' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full @@ -33,3 +40,4 @@ jobs: package-name: rtabmap_ros target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: /tmp/deps.repos + rosdep-skip-keys: "${{ matrix.skip_keys }}" diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index 67fe3c31..eb7ee5e8 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -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 && \ + rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ From 7472b019f90f08aef9fb6c8205307909231c5465 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 10 Jun 2025 11:03:57 -0700 Subject: [PATCH 120/126] Adding missing ros_enironment dep for buildfarm --- rtabmap_slam/package.xml | 2 ++ rtabmap_util/package.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 053e5840..38d38b64 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge geometry_msgs nav_msgs diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 14a75a52..eb365082 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -12,6 +12,8 @@ ament_cmake + ros_environment + cv_bridge image_transport rclcpp From d336369ca4ac4f979ed2836658137acf3b3677c2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 16 Jun 2025 19:20:21 -0700 Subject: [PATCH 121/126] Debug builtin interfaces issue (#1328) * debugging builtin_interfaces issue on arm64 * add logh * cmake debug * show arch * refactored * explicit find_library * explicitly search builtin_interfaces__rosidl_generator_c * fixing in rtabmap_slam * rtabmap_sync * openssl error * rtabmap odom * rtabmap_viz * adding missing deps instead * testing on all distros * limit fix to humble-aarch64 build * remvoed debug code * Running aarch64 workaround for all distros * message_filters error * added cmake --debug-find * using 2 threads * reseting ros2 ci --- docker/humble/latest/Dockerfile | 8 +++----- docker/jazzy/latest/Dockerfile | 4 ++-- docker/kilted/latest/Dockerfile | 4 ++-- rtabmap_conversions/CMakeLists.txt | 10 ++++++++++ rtabmap_msgs/CMakeLists.txt | 9 +++++++++ rtabmap_msgs/package.xml | 2 ++ rtabmap_odom/CMakeLists.txt | 9 +++++++++ rtabmap_odom/package.xml | 2 ++ rtabmap_rviz_plugins/CMakeLists.txt | 9 +++++++++ rtabmap_rviz_plugins/package.xml | 2 ++ rtabmap_slam/CMakeLists.txt | 9 +++++++++ rtabmap_sync/CMakeLists.txt | 14 ++++++++++++++ rtabmap_sync/package.xml | 2 ++ rtabmap_viz/CMakeLists.txt | 19 +++++++++++++++++++ rtabmap_viz/package.xml | 2 ++ 15 files changed, 96 insertions(+), 9 deletions(-) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index eb7ee5e8..d24ddaf5 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -1,19 +1,17 @@ FROM introlab3it/rtabmap:jammy -RUN source /ros_entrypoint.sh && \ - mkdir -p ros2_ws/src && \ - cd ros2_ws/src +RUN mkdir -p ros2_ws/src COPY . ros2_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j1" && \ + export MAKEFLAGS="-j2" && \ rosdep init && \ rosdep update && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 3b60aaac..e2b06208 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -8,12 +8,12 @@ COPY . ros2_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j1" && \ + export MAKEFLAGS="-j2" && \ rosdep init && \ rosdep update && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap 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 && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index 2a20a5ea..97cc6c46 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -8,12 +8,12 @@ COPY . ros2_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j1" && \ + export MAKEFLAGS="-j2" && \ rosdep init && \ rosdep update && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ 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 && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index f51f841b..3df9516a 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt index 7ae7eead..01e7cebb 100644 --- a/rtabmap_msgs/CMakeLists.txt +++ b/rtabmap_msgs/CMakeLists.txt @@ -10,6 +10,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + rcutils_LIB NAMES rcutils + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + ################## ## Dependencies ## ################## diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index ca45d59a..136fb15a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -14,6 +14,8 @@ rosidl_default_generators + ros_environment + builtin_interfaces std_msgs std_srvs diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index f9859b44..96130502 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/rtabmap_odom/CMakeLists.txt @@ -10,6 +10,15 @@ if(POLICY CMP0074) cmake_policy(SET CMP0074 NEW) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake_ros REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_geometry REQUIRED) diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index ffa9cd90..c51f592f 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_geometry laser_geometry diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index d466c1b2..c1808ebe 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -5,6 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + message_filters_LIB NAMES message_filters + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 32b779c4..ada5c080 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + pcl_conversions pluginlib rclcpp diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index d843f4f6..df0eeb6f 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -10,6 +10,15 @@ if(POLICY CMP0074) cmake_policy(SET CMP0074 NEW) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 6b45dcd7..5bbf39fd 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -1,6 +1,20 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_sync) +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + crypto_LIB NAMES crypto + PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 8eee2e6a..769155cc 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_transport message_filters diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 23b66ad8..a7707f22 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -5,6 +5,25 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + rcutils_LIB NAMES rcutils + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + crypto_LIB NAMES crypto + PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index de89ccd5..b8e2ce78 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge geometry_msgs rclcpp From aa7f42a57e86da2f6a1b86691389d44b40d86921 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 28 Jun 2025 16:46:10 -0700 Subject: [PATCH 122/126] Adding aruco msgs input support (#1334) * Adding aruco msgs input support * updated topic names --- rtabmap_slam/CMakeLists.txt | 44 +++++++ .../include/rtabmap_slam/CoreWrapper.h | 44 ++++++- rtabmap_slam/package.xml | 3 + rtabmap_slam/src/CoreWrapper.cpp | 122 ++++++++++++++++-- 4 files changed, 204 insertions(+), 9 deletions(-) diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index df0eeb6f..f53b7aaa 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -38,6 +38,10 @@ find_package(rtabmap_sync REQUIRED) #optional find_package(apriltag_msgs) +find_package(aruco_msgs) +find_package(aruco_markers_msgs) +find_package(aruco_opencv_msgs) +find_package(ros2_aruco_interfaces) find_package(nav2_msgs) IF(WIN32) @@ -88,6 +92,46 @@ SET(Libraries ) ENDIF(apriltag_msgs_FOUND) +# If aruco_msgs is found, add definition +IF(aruco_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MSGS") +SET(Libraries + ${Libraries} + aruco_msgs +) +ENDIF(aruco_msgs_FOUND) + +# If aruco_opencv_msgs is found, add definition +IF(aruco_opencv_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_opencv_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS") +SET(Libraries + ${Libraries} + aruco_opencv_msgs +) +ENDIF(aruco_opencv_msgs_FOUND) + +# If aruco_markers_msgs is found, add definition +IF(aruco_markers_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_markers_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS") +SET(Libraries + ${Libraries} + aruco_markers_msgs +) +ENDIF(aruco_markers_msgs_FOUND) + +# If ros2_aruco_interfaces is found, add definition +IF(ros2_aruco_interfaces_FOUND) +MESSAGE(STATUS "WITH ros2_aruco_interfaces") +ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES") +SET(Libraries + ${Libraries} + ros2_aruco_interfaces +) +ENDIF(ros2_aruco_interfaces_FOUND) + # If nav2_msgs is found, add definition IF(nav2_msgs_FOUND) MESSAGE(STATUS "WITH nav2_msgs") diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index a0eb6e73..157a9b26 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -88,6 +88,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef WITH_ARUCO_MSGS +#include +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +#include +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +#include +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +#include +#endif + #ifdef WITH_NAV2_MSGS #include #include @@ -178,7 +194,20 @@ private: void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection); void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections); #ifdef WITH_APRILTAG_MSGS - void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections); + void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); + void apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MSGS + void arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + void arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + void arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + void arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg); #endif #ifdef WITH_FIDUCIAL_MSGS void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections); @@ -420,6 +449,19 @@ private: rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; + rclcpp::Subscription::SharedPtr apriltagSub_; +#endif +#ifdef WITH_ARUCO_MSGS + rclcpp::Subscription::SharedPtr arucoSub_; +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + rclcpp::Subscription::SharedPtr arucoOpencvSub_; +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + rclcpp::Subscription::SharedPtr arucoMarkersSub_; +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + rclcpp::Subscription::SharedPtr arucoInterfacesSub_; #endif #ifdef WITH_FIDUCIAL_MSGS rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 38d38b64..08543237 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -27,6 +27,9 @@ tf2_ros visualization_msgs apriltag_msgs + aruco_msgs + aruco_opencv_msgs + rtabmap_msgs rtabmap_util diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index bb86ad56..c6a20333 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -886,6 +886,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + apriltagSub_ = this->create_subscription("apriltag/detections", 5, std::bind(&CoreWrapper::apriltagAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MSGS + arucoSub_ = this->create_subscription("aruco/detections", 5, std::bind(&CoreWrapper::arucoAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + arucoOpencvSub_ = this->create_subscription("aruco_opencv/detections", 5, std::bind(&CoreWrapper::arucoOpencvAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + arucoMarkersSub_ = this->create_subscription("aruco_markers/detections", 5, std::bind(&CoreWrapper::arucoMarkersAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + arucoInterfacesSub_ = this->create_subscription("aruco_interfaces/detections", 5, std::bind(&CoreWrapper::arucoInterfacesAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); @@ -2651,18 +2664,30 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm } #ifdef WITH_APRILTAG_MSGS -void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections) +void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) +{ + if(!paused_) + { + static bool warningShow = false; + if(!warningShow) { + RCLCPP_WARN(this->get_logger(), "\"tag_detections\" input topic name for apriltag_msgs is deprecated, remap \"apriltag\" input topic name instead. This message is only printed once."); + warningShow = true; + } + apriltagAsyncCallback(msg); + } +} +void CoreWrapper::apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) { if(!paused_) { UScopeMutex lock(landmarksMutex_); - for(unsigned int i=0; idetections.size(); ++i) + for(unsigned int i=0; idetections.size(); ++i) { - std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id); + std::string tagFrameId = msg->detections[i].family+":"+uNumber2Str(msg->detections[i].id); Transform camToTag = rtabmap_conversions::getTransform( - tagDetections->header.frame_id, // e.g., camera_optical_frame + msg->header.frame_id, // e.g., camera_optical_frame tagFrameId, // e.g., tag36h11:42 - tagDetections->header.stamp, + msg->header.stamp, *tfBuffer_, waitForTransform_); if(camToTag.isNull()) @@ -2670,16 +2695,97 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.", frameId_.c_str(), tagFrameId.c_str(), - tagDetections->detections[i].id); + msg->detections[i].id); continue; } geometry_msgs::msg::PoseWithCovarianceStamped p; rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose); - p.header = tagDetections->header; + p.header = msg->header; uInsert(landmarks_, - std::make_pair(tagDetections->detections[i].id, + std::make_pair(msg->detections[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MSGS +void CoreWrapper::arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose = msg->markers[i].pose; + p.header = msg->markers[i].header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +void CoreWrapper::arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].marker_id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +void CoreWrapper::arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose.pose; + p.header = msg->markers[i].pose.header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +void CoreWrapper::arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + UASSERT(msg->marker_ids.size() == msg->poses.size()); + for(unsigned int i=0; imarker_ids.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->poses[i]; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->marker_ids[i], std::make_pair(p, 0.0f))); } } From 3cc9db8f87ade67eb5a80a2a2caabfe9a83d056c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Jul 2025 17:50:38 -0700 Subject: [PATCH 123/126] Odom reset on time jump in the past (#1333) * Odom reset on time jump * Adding more logs to debug * refactored * dont skip frame on clock jump * Making clock check independent of the topic stamp check * fixed post check * Making diagnostic more robust to time jump * Added node name to warning * make sync warning msg working in case of time jump * not need to reset timer * reset timer * timer auto reset already * typo * Added more time checks to make sure we don't republish a tf frame with stamp from a topic in the future * dont send tf if time jump happened while processing * fixed errors * addressing comments * fixing time comparison --- .../include/rtabmap_odom/OdometryROS.h | 5 +- rtabmap_odom/src/OdometryROS.cpp | 106 ++++++++++++++---- rtabmap_odom/src/nodelets/icp_odometry.cpp | 12 +- rtabmap_slam/src/CoreWrapper.cpp | 18 +++ .../include/rtabmap_sync/SyncDiagnostic.h | 27 ++++- 5 files changed, 136 insertions(+), 32 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index c5672d36..24d20160 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -87,7 +87,7 @@ protected: tf::TransformListener & tfListener() {return tfListener_;} double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;} rtabmap::Transform velocityGuess() const; - double previousStamp() const {return previousStamp_;} + ros::Time previousStamp() const {return previousStamp_;} virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {} private: @@ -158,7 +158,8 @@ private: bool icpParams_; rtabmap::Transform guess_; rtabmap::Transform guessPreviousPose_; - double previousStamp_; + ros::Time previousStamp_; + ros::Time previousClockTime_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f79fca44..f5b54ba9 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -78,7 +78,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : stereoParams_(stereoParams), visParams_(visParams), icpParams_(icpParams), - previousStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), @@ -543,27 +542,56 @@ void OdometryROS::mainLoop() Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { - if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec()) + // Detect time jump in the past + ros::Time clockNow = ros::Time::now(); + if(previousClockTime_ > clockNow) + { + NODELET_WARN("Odometry: Detected jump back in time of %f sec. Odometry is " + "automatically reset to latest computed pose!", + (previousClockTime_ - clockNow).toSec()); + SensorData dataCpy = dataToProcess_; + std_msgs::Header headerCpy = dataHeaderToProcess_; + ros::Time previousCpy = previousClockTime_; + this->reset(odometry_->getPose()); + if(previousCpy > headerCpy.stamp) { + // new frame is using new clock, process it now + dataToProcess_ = dataCpy; + dataHeaderToProcess_ = headerCpy; + dataReady_.release(); + NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + else { + // skip that old frame + NODELET_WARN("Odometry: skipping frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + previousClockTime_ = clockNow; + return; + } + previousClockTime_ = clockNow; + + if(previousStamp_ >= header.stamp) { NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). " - "New stamp should be always greater than previous stamp. This new data is ignored.", - previousStamp_, header.stamp.toSec()); + "New stamp should be always greater than previous stamp. This new data is ignored. ", + previousStamp_.toSec(), header.stamp.toSec()); return; } else if(maxUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) + previousStamp_.toSec() > 0 && + ((header.stamp-previousStamp_).toSec()+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) { // throttling return; } else if(maxUpdateRate_ == 0 && expectedUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_) + previousStamp_.toSec() > 0 && + (header.stamp-previousStamp_).toSec() < 1.0/expectedUpdateRate_) { NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)", - 1.0/(header.stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, header.stamp.toSec()); + 1.0/(header.stamp-previousStamp_).toSec(), expectedUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); return; } @@ -632,7 +660,7 @@ void OdometryROS::mainLoop() guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) && (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) && - (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && header.stamp.toSec()-previousStamp_ < guessMinTime_))) + (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_))) { // Ignore odometry update, we didn't move enough if(publishTf_) @@ -643,7 +671,16 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } guessPreviousPose_ = guessCurrentPose; return; @@ -658,7 +695,7 @@ void OdometryROS::mainLoop() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (header.stamp.toSec()-previousStamp_) > 1.0/minUpdateRate_; + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; // process data ros::WallTime time = ros::WallTime::now(); @@ -697,11 +734,29 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } else { - tfBroadcaster_.sendTransform(poseMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(poseMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + poseMsg.header.frame_id.c_str(), + poseMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } } @@ -893,7 +948,18 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because its stamp (%f) is greater " + "than current time (%f), possible time jump happened!", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + correctionMsg.header.stamp.toSec(), + time_now.toSec()); + } } } @@ -903,7 +969,7 @@ void OdometryROS::mainLoop() { NODELET_WARN( "Odometry lost! Odometry will be reset because last update " "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); + (header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); } else if(--resetCurrentCount_>0) { @@ -1098,10 +1164,10 @@ void OdometryROS::mainLoop() syncDiagnostic_->tick(header.stamp, maxUpdateRate_>0 ? maxUpdateRate_: expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); + previousStamp_.toSec() == 0.0 || (header.stamp - previousStamp_).toSec() > 1.0/curentRate?0:curentRate); } - previousStamp_ = header.stamp.toSec(); + previousStamp_ = header.stamp; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) @@ -1125,7 +1191,8 @@ void OdometryROS::reset(const Transform & pose) odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); - previousStamp_ = 0.0; + previousStamp_ = ros::Time(); + previousClockTime_ = ros::Time(); resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); @@ -1135,6 +1202,7 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); + this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 23bb280e..1c712e1c 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -380,11 +380,11 @@ private: -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); - if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull()) + if(guessFrameId().empty() && previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model (we are in frameId) sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -405,11 +405,11 @@ private: { projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); - if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull()) + if(deskewing_ && previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -628,7 +628,7 @@ private: return; } } - else if(previousStamp() > 0 && !velocityGuess().isNull()) + else if(previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0; @@ -648,7 +648,7 @@ private: } sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2); - if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 6a3ab207..ff930a44 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,6 +1023,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1134,6 +1143,15 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index dfc3e384..59f9181f 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -19,7 +19,9 @@ class SyncDiagnostic { compositeTask_("Sync status"), lastCallbackCalledStamp_(ros::Time::now().toSec()-1), targetFrequency_(0.0), - windowSize_(windowSize) + windowSize_(windowSize), + lastTickTime_(0.0), + nodeName_(nodeName) { UASSERT(windowSize_ >= 1); } @@ -46,7 +48,7 @@ class SyncDiagnostic { } diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/")); diagnosticUpdater_.force_update(); - diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this); + diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(5), &SyncDiagnostic::diagnosticTimerCallback, this); } void tick(const ros::Time & stamp, double targetFrequency = 0) @@ -79,16 +81,29 @@ class SyncDiagnostic { targetFrequency_ = targetFrequency; } lastCallbackCalledStamp_ = stamp.toSec(); + + double clockNow = ros::Time::now().toSec(); + if(lastTickTime_ > clockNow) + { + ROS_WARN("%s: Detected time jump in the past of %f sec, forcing diagnostic update.", + nodeName_.c_str(), lastTickTime_ - clockNow); + frequencyStatus_.clear(); + diagnosticUpdater_.force_update(); + lastCallbackCalledStamp_ = clockNow; + } + else + { + diagnosticUpdater_.update(); + } + lastTickTime_ = clockNow; } private: void diagnosticTimerCallback(const ros::TimerEvent& event) { - diagnosticUpdater_.update(); - if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) { - ROS_WARN_THROTTLE(5, "%s", topicsNotReceivedWarningMsg_.c_str()); + ROS_WARN("%s", topicsNotReceivedWarningMsg_.c_str()); } } @@ -103,6 +118,8 @@ private: double targetFrequency_; int windowSize_; std::deque window_; + double lastTickTime_; + std::string nodeName_; }; From 0ba59996e695ade7b20a182fbce0bc447537cc86 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 22:01:22 +0000 Subject: [PATCH 124/126] refactored time jump detection --- rtabmap_odom/src/OdometryROS.cpp | 1 - rtabmap_slam/src/CoreWrapper.cpp | 18 ------------------ 2 files changed, 19 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f5b54ba9..81d08076 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -1202,7 +1202,6 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); - this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index ff930a44..6a3ab207 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,15 +1023,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1143,15 +1134,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) From bc8123089e3f28aadadc61bcae050df25ff20039 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 16:59:24 -0700 Subject: [PATCH 125/126] Added new demo turtlebot3_sim_rgbd_fake_scan_demo.launch.py --- .../turtlebot3_rgbd_fake_scan.launch.py | 143 ++++++++++++++++++ ...rtlebot3_sim_rgbd_fake_scan_demo.launch.py | 117 ++++++++++++++ .../src/nodelets/point_cloud_assembler.cpp | 2 +- 3 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py new file mode 100644 index 00000000..1ccacd65 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py @@ -0,0 +1,143 @@ +# Example: +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py +# +# SLAM: +# $ ros2 launch rtabmap_demos turtlebot3_rgbd_fake_scan.launch.py +# +# Navigation (install nav2_bringup package): +# $ ros2 launch nav2_bringup navigation_launch.py +# $ ros2 launch nav2_bringup rviz_launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'use_sim_time':use_sim_time, + 'subscribe_rgbd':True, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'scan_cloud_is_2d': True, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + + remappings=[ + ('rgb/image', '/camera/image_raw'), + ('rgb/camera_info', '/camera/camera_info'), + ('depth/image', '/camera/depth/image_raw'), + ('scan_cloud', 'assembled_cloud')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + # Convert middle row of depth pixels to a fake laser scan + Node( + package='depthimage_to_laserscan', executable='depthimage_to_laserscan_node', output='screen', + parameters=[{ + 'use_sim_time':use_sim_time, + 'range_max': 5.0 + }], + remappings=[ + ('depth', '/camera/depth/image_raw'), + ('depth_camera_info', '/camera/camera_info'), + ('scan', '/camera/scan') + ]), + + # Just to convert the fake laser scan to PointCloud2 + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{'use_sim_time':use_sim_time, + 'fixed_frame_id': 'camera_link'}], # use camera frame + remappings=[ + ('input_scan', '/camera/scan') + ]), + + # Assemble the fake laser scans using a circular buffer, then feed that cloud to rtabmap + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + parameters=[{'use_sim_time':use_sim_time, + 'max_clouds': 20, + 'voxel_size': 0.05, + 'wait_for_transform': 1.0, + 'linear_update': 0.3, + 'angular_update': 0.5, + 'circular_buffer': True, + 'frame_id': 'base_link'}], + remappings=[ + ('assembled_cloud', 'assembled_cloud'), + ('cloud', '/camera/scan/deskewed') + ]), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py new file mode 100644 index 00000000..314ad3eb --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py @@ -0,0 +1,117 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 2edf5509..99b64ccf 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -130,7 +130,7 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : if(maxClouds_==0 && assemblingTime_ ==0.0) { - RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!"); + RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_clouds or assembling_time parameters should be set!"); exit(-1); } From 0918c60dc1e276a2cf9d2ca800131d20ed1e442b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 17:20:25 -0700 Subject: [PATCH 126/126] Update README.md --- rtabmap_demos/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index b1eb14ce..4f37769c 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -7,6 +7,7 @@ + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam) + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) @@ -50,6 +51,14 @@ [turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py) ![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783) +### Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM +[turtlebot3_sim_rgbd_fake_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py) + + * Red: Scan generated from camera's depth. + * Orange: Locally assembled scans used for proximity detection. + * Yellow: The map. + +![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1) ### Champ Quadruped Nav2, Elevation Map and VSLAM [champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py)