From 659fee9a261c99e9e14975858419b00399600757 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 28 May 2024 10:56:27 -0700 Subject: [PATCH 01/35] 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 02/35] 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 03/35] 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 04/35] 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 05/35] 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 06/35] 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 07/35] 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 08/35] 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 09/35] 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 10/35] 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 11/35] 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 12/35] 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 13/35] 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 14/35] 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 15/35] 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 16/35] 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 17/35] 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 18/35] 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 19/35] 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 20/35] 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 21/35] 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 22/35] 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 23/35] 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 24/35] 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 25/35] 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 26/35] 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 27/35] 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 28/35] 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 29/35] 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 30/35] 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 31/35] 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 32/35] 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 33/35] 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 34/35] 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 35/35] 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