diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 00000000..62978bb7 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -0,0 +1,17 @@ + +FROM introlab3it/rtabmap:jammy + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} + +RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ + chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws + +RUN echo "source /opt/ros/humble/setup.bash" >> /home/${USERNAME}/.bashrc +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json new file mode 100644 index 00000000..a1666586 --- /dev/null +++ b/.devcontainer/humble/devcontainer.json @@ -0,0 +1,27 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "pull": true + }, + "remoteUser": "vscode", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-themes", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "ms-azuretools.vscode-docker", + "ms-python.python"] + } + }, + "settings": { + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/home/vscode/ros2_ws", + "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" + //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] +} diff --git a/.devcontainer/jazzy/Dockerfile b/.devcontainer/jazzy/Dockerfile new file mode 100644 index 00000000..13b36c5a --- /dev/null +++ b/.devcontainer/jazzy/Dockerfile @@ -0,0 +1,20 @@ + +FROM introlab3it/rtabmap:noble + +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} + +RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ + chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws + +RUN echo "source /opt/ros/jazzy/setup.bash" >> /home/${USERNAME}/.bashrc +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json new file mode 100644 index 00000000..5e768e9a --- /dev/null +++ b/.devcontainer/jazzy/devcontainer.json @@ -0,0 +1,27 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "pull": true + }, + "remoteUser": "vscode", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-themes", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "ms-azuretools.vscode-docker", + "ms-python.python"] + } + }, + "settings": { + "python.autoComplete.extraPaths": [ + "/opt/ros/jazzy/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/home/vscode/ros2_ws", + "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" + //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] +} diff --git a/.devcontainer/kilted/Dockerfile b/.devcontainer/kilted/Dockerfile new file mode 100644 index 00000000..9aee0a83 --- /dev/null +++ b/.devcontainer/kilted/Dockerfile @@ -0,0 +1,20 @@ + +FROM introlab3it/rtabmap:noble-kilted + +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} + +RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ + chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws + +RUN echo "source /opt/ros/kilted/setup.bash" >> /home/${USERNAME}/.bashrc +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/kilted/devcontainer.json b/.devcontainer/kilted/devcontainer.json new file mode 100644 index 00000000..6176ee77 --- /dev/null +++ b/.devcontainer/kilted/devcontainer.json @@ -0,0 +1,27 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "pull": true + }, + "remoteUser": "vscode", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-themes", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "ms-azuretools.vscode-docker", + "ms-python.python"] + } + }, + "settings": { + "python.autoComplete.extraPaths": [ + "/opt/ros/kilted/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/home/vscode/ros2_ws", + "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" + //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] +} diff --git a/.devcontainer/noetic/Dockerfile b/.devcontainer/noetic/Dockerfile new file mode 100644 index 00000000..3eec0a03 --- /dev/null +++ b/.devcontainer/noetic/Dockerfile @@ -0,0 +1,17 @@ + +FROM introlab3it/rtabmap:focal + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} + +RUN mkdir -p /home/${USERNAME}/catkin_ws/src && \ + chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/catkin_ws + +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USERNAME}/.bashrc +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/noetic/devcontainer.json b/.devcontainer/noetic/devcontainer.json new file mode 100644 index 00000000..4990bced --- /dev/null +++ b/.devcontainer/noetic/devcontainer.json @@ -0,0 +1,27 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "pull": true + }, + "remoteUser": "vscode", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-themes", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "ms-azuretools.vscode-docker", + "ms-python.python"] + } + }, + "settings": { + "python.autoComplete.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/home/vscode/catkin_ws", + "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace" + //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] +} diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 27033771..72a91300 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -10,8 +10,9 @@ jobs: runs-on: ubuntu-latest strategy: + fail-fast: false matrix: - docker_tag: [humble, humble-latest, iron, iron-latest] + docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted-latest] include: - docker_tag: humble docker_path: 'humble' @@ -22,42 +23,49 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 - - docker_tag: iron - docker_path: 'iron' + - docker_tag: jazzy + docker_path: 'jazzy' docker_platforms: | linux/amd64 - - docker_tag: iron-latest - docker_path: 'iron/latest' + linux/arm64 + - docker_tag: jazzy-latest + docker_path: 'jazzy/latest' docker_platforms: | linux/amd64 + linux/arm64 + - docker_tag: kilted-latest + docker_path: 'kilted/latest' + docker_platforms: | + linux/amd64 + linux/arm64 steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Set up QEMU - uses: docker/setup-qemu-action@v1 + uses: docker/setup-qemu-action@v3 with: platforms: all - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v3 - name: Login to DockerHub - uses: docker/login-action@v1 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and push - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v6 with: context: . push: true platforms: ${{ matrix.docker_platforms }} file: ./docker/${{ matrix.docker_path }}/Dockerfile tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }} - cache-from: type=registry,ref=introlab3it/rtabmap_ros:${{ matrix.docker_tag }} + no-cache: true cache-to: type=inline diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index ec6913e1..ab73b32e 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -32,7 +32,6 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 - linux/arm/v7 steps: - diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml deleted file mode 100644 index 39f25e63..00000000 --- a/.github/workflows/ros1.yml +++ /dev/null @@ -1,62 +0,0 @@ -name: ros1 - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) - BUILD_TYPE: Release - -jobs: - build: - # The CMake configure and build commands are platform agnostic and should work equally - # well on Windows or Mac. You can convert this to a matrix build if you need - # cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-20.04] - include: - - os: ubuntu-20.04 - ros_distro: 'noetic' - - - steps: - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ matrix.ros_distro }} - - - name: Install dependencies - run: | - sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools - sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap - sudo pip3 uninstall empy --yes - - - name: Setup catkin workspace - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - mkdir -p ${{github.workspace}}/catkin_ws/src - cd ${{github.workspace}}/catkin_ws/src - cd .. - catkin config --init --cmake-args -DSETUPTOOLS_DEB_LAYOUT=OFF -DCMAKE_C_FLAGS="-Wformat -Werror=format-security" -DCMAKE_CXX_FLAGS="-Wformat -Werror=format-security" - - - uses: actions/checkout@v2 - with: - repository: 'introlab/rtabmap' - path: 'catkin_ws/src/rtabmap' - - - uses: actions/checkout@v2 - with: - path: 'catkin_ws/src/rtabmap_ros' - - - name: caktkin build - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - cd ${{github.workspace}}/catkin_ws - catkin build -p 1 -i --verbose diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index b810295b..790f78c4 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -8,51 +8,36 @@ on: branches: [ ros2 ] env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: - # The CMake configure and build commands are platform agnostic and should work equally - # well on Windows or Mac. You can convert this to a matrix build if you need - # cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} + name: Build ros2 ${{ matrix.ros_distro }} + runs-on: ubuntu-latest strategy: matrix: - ros_distro: [humble, iron] + ros_distro: [humble, jazzy, kilted] include: - - ros_distro: 'humble' - os: ubuntu-22.04 - - ros_distro: 'iron' - os: ubuntu-22.04 - + - ros_distro: humble + skip_keys: '' + - ros_distro: jazzy + skip_keys: '' + - ros_distro: kilted + skip_keys: 'nav2_bringup nav2_msgs' + fail-fast: false + container: + image: osrf/ros:${{ matrix.ros_distro }}-desktop-full steps: - - uses: ros-tooling/setup-ros@v0.6 + - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} - - - name: Setup ros2 workspace - 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 + - run: | + echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\ + cat /tmp/deps.repos + - uses: ros-tooling/action-ros-ci@v0.4 with: - repository: 'introlab/rtabmap' - path: 'ros2_ws/src/rtabmap' - - - uses: actions/checkout@v2 - with: - path: 'ros2_ws/src/rtabmap_ros' - - - name: colcon build - 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 }} + vcs-repo-file-url: /tmp/deps.repos + rosdep-skip-keys: "${{ matrix.skip_keys }}" diff --git a/.gitignore b/.gitignore index 7feaafd2..5fb548e9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .pydevproject .settings +__pycache__ diff --git a/README.md b/README.md index 87fa8918..de5a2176 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ rtabmap_ros =========== -RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2, however they are not all tested yet. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). +RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Humble minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). #### CI Latest @@ -30,17 +30,17 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current Build Status - ROS 2 + ROS 2 Humble Build Status - Iron - Build Status + Jazzy + Build Status Rolling - Build Status + Build Status Docker @@ -54,41 +54,27 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current # Usage -`rtabmap.launch` is also ported to ROS2 with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this: +* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder. +* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder. + +## Logging +To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info): ```bash -roslaunch zed_wrapper zed_no_tf.launch - -roslaunch rtabmap_ros rtabmap.launch \ - rtabmap_args:="--delete_db_on_start" \ - rgb_topic:=/zed/zed_node/rgb/image_rect_color \ - depth_topic:=/zed/zed_node/depth/depth_registered \ - camera_info_topic:=/zed/zed_node/rgb/camera_info \ - frame_id:=base_link \ - approx_sync:=false \ - wait_imu_to_init:=true \ - imu_topic:=/zed_node/imu/data - +export RCUTILS_LOGGING_USE_STDOUT=1 +export RCUTILS_LOGGING_BUFFERED_STREAM=1 +# Optional, but if you like colored logs: +export RCUTILS_COLORIZED_OUTPUT=1 ``` -The ROS2 equivalent is (with those [lines](https://github.com/stereolabs/zed-ros2-wrapper/blob/b512dce6ad4565f4770273995b147122e735ca0f/zed_wrapper/config/common.yaml#L58-L60) set to false to avoid TF conflicts): - +## Recommended DDS +If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`): ```bash -ros2 launch zed_wrapper zed.launch.py - -ros2 launch rtabmap_launch rtabmap.launch.py \ - rtabmap_args:="--delete_db_on_start" \ - rgb_topic:=/zed/zed_node/rgb/image_rect_color \ - depth_topic:=/zed/zed_node/depth/depth_registered \ - camera_info_topic:=/zed/zed_node/rgb/camera_info \ - frame_id:=base_link \ - approx_sync:=false \ - wait_imu_to_init:=true \ - imu_topic:=/zed/zed_node/imu/data \ - qos:=1 \ - rviz:=true +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# Cyclone prefers multicast by default, if your router got too much spammed, +# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489): +export CYCLONEDDS_URI="0.0.0.0" ``` -`qos` (Quality of Service) argument should match the published topics QoS (1=RELIABLE, 2=BEST EFFORT). ROS1 was always RELIABLE. # Installation @@ -117,39 +103,3 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release ``` -# Example with Turtlebot3 - -1. Launch Turtlebot3 simulator: - ```bash - export TURTLEBOT3_MODEL=waffle - ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py - - export TURTLEBOT3_MODEL=waffle - ros2 run turtlebot3_teleop teleop_keyboard - ``` - -2. Launch RTAB-Map: - ``` - ros2 launch rtabmap_demos turtlebot3_scan.launch.py - - # OR with rtabmap.launch.py - ros2 launch rtabmap_launch rtabmap.launch.py \ - visual_odometry:=false \ - frame_id:=base_footprint \ - subscribe_scan:=true depth:=false \ - approx_sync:=true \ - odom_topic:=/odom \ - scan_topic:=/scan \ - qos:=2 \ - args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \ - use_sim_time:=true \ - rviz:=true - ``` - -3. Launch navigation (`nav2_bringup` package should be installed): - ``` - ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True - ros2 launch nav2_bringup rviz_launch.py - ``` - -See [rtabmap_demos/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos/launch) and [rtabmap_examples/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) subfolders for some other ROS2 examples with turtlebot3 in simulation and a RGB-D camera. diff --git a/docker/README.md b/docker/README.md index 7774f8a2..6714f614 100644 --- a/docker/README.md +++ b/docker/README.md @@ -2,8 +2,8 @@ * Available images on [introlab3it/rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros/): ``` - foxy, foxy-latest humble, humble-latest + jazzy, jazzy-latest ``` * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. @@ -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/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index b7d01701..d24ddaf5 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -1,20 +1,17 @@ -FROM introlab3it/rtabmap:22.04 +FROM introlab3it/rtabmap:jammy -RUN source /ros_entrypoint.sh && \ - mkdir -p ros2_ws/src && \ - cd ros2_ws/src +RUN mkdir -p ros2_ws/src COPY . ros2_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j1" && \ + export MAKEFLAGS="-j2" && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ - apt remove ros-$ROS_DISTRO-rtabmap* -y && \ + rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/humble/latest/hooks/build b/docker/humble/latest/hooks/build deleted file mode 100644 index bffa442b..00000000 --- a/docker/humble/latest/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:humble-latest . diff --git a/docker/jazzy/Dockerfile b/docker/jazzy/Dockerfile new file mode 100644 index 00000000..b845aa95 --- /dev/null +++ b/docker/jazzy/Dockerfile @@ -0,0 +1,6 @@ +FROM osrf/ros:jazzy-desktop +# install rtabmap packages +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..e2b06208 --- /dev/null +++ b/docker/jazzy/latest/Dockerfile @@ -0,0 +1,19 @@ +FROM introlab3it/rtabmap:noble + +RUN source /ros_entrypoint.sh && \ + mkdir -p ros2_ws/src && \ + cd ros2_ws/src + +COPY . ros2_ws/src/rtabmap_ros + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ + rosdep init && \ + rosdep update && \ + apt-get update && \ + rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + cd && \ + rm -rf ros2_ws diff --git a/docker/kilted/Dockerfile b/docker/kilted/Dockerfile new file mode 100644 index 00000000..6c0ba3b3 --- /dev/null +++ b/docker/kilted/Dockerfile @@ -0,0 +1,6 @@ +FROM osrf/ros:kilted-desktop +# install rtabmap packages +RUN apt-get update && apt-get install -y \ + ros-kilted-rtabmap \ + ros-kilted-rtabmap-ros \ + && rm -rf /var/lib/apt/lists/ diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile new file mode 100644 index 00000000..97cc6c46 --- /dev/null +++ b/docker/kilted/latest/Dockerfile @@ -0,0 +1,19 @@ +FROM introlab3it/rtabmap:noble-kilted + +RUN source /ros_entrypoint.sh && \ + mkdir -p ros2_ws/src && \ + cd ros2_ws/src + +COPY . ros2_ws/src/rtabmap_ros + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ + rosdep init && \ + rosdep update && \ + apt-get update && \ + rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + cd && \ + rm -rf ros2_ws diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 27bb3b22..3df9516a 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) @@ -19,7 +29,7 @@ find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(RTABMap 0.21.5 REQUIRED) +find_package(RTABMap 0.22.0 REQUIRED) include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -46,6 +56,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 +72,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..3d9f5daa 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 @@ -319,6 +323,42 @@ inline int sizeOfPointField(int datatype) } return -1; } + +template +typename std::map::const_iterator getClosestIterator( + const std::map & buffer, + const K & key) +{ + UASSERT(!buffer.empty()); + typename std::map::const_iterator iterB = buffer.lower_bound(key); + typename std::map::const_iterator iterA = iterB; + if(iterA != buffer.begin()) + { + iterA = --iterA; + } + if(iterB == buffer.end()) + { + iterB = --iterB; + } + if(iterA == iterB) + { + return iterA; + } + if(iterA->first > key) + { + return iterA; + } + else if(iterB->first < key) + { + return iterB; + } + else if(key - iterA->first < iterB->first - key) + { + return iterA; + } + return iterB; +} + } #endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 6aa39f82..3a9484cf 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -2,7 +2,7 @@ rtabmap_conversions - 0.21.5 + 0.22.0 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 1587d06a..3c97a2c4 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 @@ -918,7 +923,8 @@ void cameraModelToROS( UASSERT(model.R().empty() || model.R().total() == 9); if(model.R().empty()) { - memset(camInfo.r.data(), 0.0, 9*sizeof(double)); + cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1); + memcpy(camInfo.r.data(), eye.data, 9*sizeof(double)); } else { @@ -929,6 +935,10 @@ void cameraModelToROS( if(model.P().empty()) { memset(camInfo.p.data(), 0.0, 12*sizeof(double)); + if(!model.K_raw().empty()) { + model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3)); + camInfo.p.back() = 1.0; + } } else { @@ -1610,6 +1620,8 @@ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers)); stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints)); stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f)); + stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", info.localBundleAvgInlierDistance)); + stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", info.localBundleMaxKeyFramesForInlier)); stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f)); stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval)); stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled)); @@ -1640,9 +1652,8 @@ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & { if(!info.transform.isNull()) { - rtabmap::Transform diff = info.transformGroundTruth.inverse()*info.transform; - stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm())); - stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI)); + stats.insert(std::make_pair("Odometry/TG_error_lin/m", info.transformGroundTruth.getDistance(info.transform))); + stats.insert(std::make_pair("Odometry/TG_error_ang/deg", info.transformGroundTruth.getAngle(info.transform)*180.0/CV_PI)); } info.transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); @@ -1686,18 +1697,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b info.localBundleOutliers = msg.local_bundle_outliers; info.localBundleConstraints = msg.local_bundle_constraints; info.localBundleTime = msg.local_bundle_time; - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size()); - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size()); - for(size_t i=0; i models; - for(size_t j=0; j models; + for(size_t j=0; j >::const_iterator iter=info.localBundleModels.begin(); iter!=info.localBundleModels.end(); @@ -1983,12 +1999,6 @@ rtabmap::Transform getTransform( { // TF ready? rtabmap::Transform transform; - std::string errString; - if(!tfBuffer.canTransform(fromFrameId, toFrameId, tf2_ros::fromMsg(stamp), tf2::durationFromSec(waitForTransform), &errString)) - { - UWARN("(can transform %s -> %s?) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), errString.c_str(), waitForTransform); - return rtabmap::Transform(); - } try { geometry_msgs::msg::TransformStamped tmp; @@ -2169,7 +2179,7 @@ bool convertRGBDMsgs( rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform); if(localTransform.isNull()) { - UERROR("TF of received image %d at time %fs is not set!", i, stamp.seconds()); + UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds()); return false; } // sync with odometry stamp @@ -2588,6 +2598,24 @@ bool convertScanMsg( double waitForTransform, bool outputInFrameId) { + // scan message validation check + if(scan2dMsg.angle_increment == 0.0f) { + UERROR("convertScanMsg: angle_increment should not be 0!"); + return false; + } + if(scan2dMsg.range_min > scan2dMsg.range_max) { + UERROR("convertScanMsg: range_min (%f) should be smaller than range_max (%f)!", scan2dMsg.range_min, scan2dMsg.range_max); + return false; + } + if(scan2dMsg.angle_increment > 0 && scan2dMsg.angle_max < scan2dMsg.angle_min) { + UERROR("convertScanMsg: Angle increment (%f) should be negative if angle_min(%f) > angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + else if (scan2dMsg.angle_increment < 0 && scan2dMsg.angle_max > scan2dMsg.angle_min) { + UERROR("convertScanMsg: Angle increment (%f) should positive if angle_min(%f) < angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = getMovingTransform( scan2dMsg.header.frame_id, @@ -3035,6 +3063,25 @@ bool deskew_impl( } } + if(secFirst > 1.e18) + { + // convert nanoseconds to seconds + secFirst /= 1.e9; + secLast /= 1.e9; + } + else if(secFirst > 1.e15) + { + // convert microseconds to seconds + secFirst /= 1.e6; + secLast /= 1.e6; + } + else if(secFirst > 1.e12) + { + // convert milliseconds to seconds + secFirst /= 1.e3; + secLast /= 1.e3; + } + firstStamp = timestampToROS(secFirst); lastStamp = timestampToROS(secLast); } @@ -3173,6 +3220,21 @@ bool deskew_impl( else if(timeDatatype == 8) //float64 { double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = timestampToROS(sec); } @@ -3252,6 +3314,21 @@ bool deskew_impl( else if(timeDatatype == 8) { double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = timestampToROS(sec); } @@ -3308,7 +3385,7 @@ bool deskew_impl( } } } - UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); + UDEBUG("Lidar deskewing time=%fs (slerp=%s waitForTransform=%f)", processingTime.elapsed(), slerp?"true":"false", waitForTransform); return true; } diff --git a/rtabmap_demos/CMakeLists.txt b/rtabmap_demos/CMakeLists.txt index 2e7ce52f..4c662b4c 100644 --- a/rtabmap_demos/CMakeLists.txt +++ b/rtabmap_demos/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_demos) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch config params data DESTINATION share/${PROJECT_NAME} ) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md new file mode 100644 index 00000000..4f37769c --- /dev/null +++ b/rtabmap_demos/README.md @@ -0,0 +1,85 @@ +# rtabmap_demos ++ [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) ++ [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) ++ [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) ++ [Find-Object with SLAM](#find-object-with-slam) ++ [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) ++ [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) ++ [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam) ++ [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) ++ [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-assembling-and-rgb-d-slam) ++ [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) ++ [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) + +### Outdoor Stereo VSLAM +[stereo_outdoor_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/stereo_outdoor_demo.launch.py) ([Video](https://youtu.be/qpTS7kg9J3A)) + +![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23) + +### Indoor 2D LiDAR and RGB-D SLAM +[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py) (Videos: [rtabmap_viz](https://youtu.be/c0qrEd5rR7M), [rviz](https://youtu.be/MQoSDpAsqps)) + +![Peek 2024-11-29 11-07](https://github.com/user-attachments/assets/b02beeea-28ed-4fde-932d-c89bef1a046d) + +### Multi-Session Indoor 2D LiDAR and RGB-D SLAM +[multisession_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/multisession_mapping_demo.launch.py) ([Video](https://youtu.be/XrnyhaxPCro)) + +![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b) + +### Find-Object with SLAM +[find_object_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/find_object_demo.launch.py) ([Video](https://youtu.be/o1GSQanY-Do)) + +![Peek 2024-11-29 12-01](https://github.com/user-attachments/assets/b3cc0c67-517a-4f69-b4cc-35d288e96165) + +### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM +[turtlebot4_sim_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py) + +![Peek 2024-11-29 12-19](https://github.com/user-attachments/assets/5914e34c-19f1-4b7c-b4df-2e7084946888) +### Turtlebot3 Nav2 and 2D LiDAR SLAM +[turtlebot3_sim_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py) + +![Peek 2024-11-29 12-23](https://github.com/user-attachments/assets/e3c31c5a-5c46-4370-ad17-38c795db7917) +### Turtlebot3 Nav2 and RGB-D SLAM +[turtlebot3_sim_rgbd_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py) + +![Peek 2024-11-29 14-22](https://github.com/user-attachments/assets/5088be17-0875-42cc-b863-d14468c67f26) +### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM +[turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py) + +![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783) +### Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM +[turtlebot3_sim_rgbd_fake_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py) + + * Red: Scan generated from camera's depth. + * Orange: Locally assembled scans used for proximity detection. + * Yellow: The map. + +![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1) +### Champ Quadruped Nav2, Elevation Map and VSLAM +[champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py) + +![Peek 2024-11-29 15-00](https://github.com/user-attachments/assets/d1a27c78-27bc-4901-82a7-59b5d24e6454) +### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM +[husky_sim_scan2d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py) + +![Peek 2024-11-29 15-30](https://github.com/user-attachments/assets/c8f79b86-253e-4c8e-ac7a-c26584f43fa4) +### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM +[husky_sim_scan3d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py) + +![Peek 2024-11-29 15-36](https://github.com/user-attachments/assets/a4b6e6ae-38ed-44da-bbfb-d3c30a301f9c) +### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM +[husky_sim_scan3d_assemble_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py) + +![Peek 2024-11-29 16-16](https://github.com/user-attachments/assets/b2235bd2-33d2-4c44-b6e9-9923a524632b) +### Isaac Sim Nav2 and Stereo SLAM +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) + +![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) +### Isaac Sim Nav2 and RGB-D VSLAM +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) stereo:=false vo:=rtabmap + +![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) diff --git a/rtabmap_demos/config/demo_robot_mapping.rviz b/rtabmap_demos/config/demo_robot_mapping.rviz new file mode 100644 index 00000000..40e28170 --- /dev/null +++ b/rtabmap_demos/config/demo_robot_mapping.rviz @@ -0,0 +1,391 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 627 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: MapCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + az3_base_link: + Value: true + az3_odom: + Value: true + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + stereo_camera: + Value: true + stereo_camera_base: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + stereo_camera_base: + stereo_camera: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 237; 51; 59 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /jn0/base_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rtabmap_rviz_plugins/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Landmark: 0; 128; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapGraph + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Current Frame + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Feature Map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: VO + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.2877197265625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8703982830047607 + Target Frame: base_footprint + Value: Orbit (rviz) + Yaw: 3.7535834312438965 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006330000003efc0100000002fb0000000800540069006d0065010000000000000633000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c2000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1587 + X: 214 + Y: 77 diff --git a/rtabmap_demos/config/find_object.ini b/rtabmap_demos/config/find_object.ini new file mode 100644 index 00000000..515dbdf9 --- /dev/null +++ b/rtabmap_demos/config/find_object.ini @@ -0,0 +1,188 @@ +[General] +windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x4\x34\0\0\x1\x8e\0\0\x6\x9a\0\0\x4\x5\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5\0\0\0\0\0\0\0\0\a\x80\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5) +windowState=@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xda\0\0\x2'\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\x1\0\0\0\x16\0\0\x2'\0\0\0\xc4\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x1\x86\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x16\0\0\x1\x86\0\0\0\xa8\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x35\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x87\0\0\x2'\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0) + +[Camera] +1deviceId=0 +2imageWidth=640 +3imageHeight=480 +4imageRate=0 +5mediaPath= +6useTcpCamera=false +7IP=127.0.0.1 +8port=5000 +9queueSize=1 + +[Feature2D] +1Detector="5:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK;AGAST;KAZE;AKAZE;SuperPointTorch" +2Descriptor="2:Brief;ORB;SIFT;SURF;BRISK;FREAK;KAZE;AKAZE;LUCID;LATCH;DAISY;SuperPointTorch" +3MaxFeatures=0 +4Affine=false +5AffineCount=6 +6SubPix=false +7SubPixWinSize=3 +8SubPixIterations=30 +9SubPixEps=0.02 +AGAST_nonmaxSuppression=true +AGAST_threshold=10 +AKAZE_descriptorChannels=3 +AKAZE_descriptorSize=0 +AKAZE_nOctaveLayers=4 +AKAZE_nOctaves=4 +AKAZE_threshold=0.001 +BRISK_octaves=3 +BRISK_patternScale=1 +BRISK_thresh=30 +Brief_bytes=32 +DAISY_interpolation=true +DAISY_q_hist=8 +DAISY_q_radius=3 +DAISY_q_theta=8 +DAISY_radius=15 +DAISY_use_orientation=false +Dense_featureScaleLevels=1 +Dense_featureScaleMul=0.1 +Dense_initFeatureScale=1 +Dense_initImgBound=0 +Dense_initXyStep=6 +Dense_varyImgBoundWithScale=false +Dense_varyXyStepWithScale=true +FREAK_nOctaves=4 +FREAK_orientationNormalized=true +FREAK_patternScale=22 +FREAK_scaleNormalized=true +Fast_gpu=false +Fast_keypointsRatio=0.05 +Fast_maxNpoints=5000 +Fast_nonmaxSuppression=true +Fast_threshold=10 +GFTT_blockSize=3 +GFTT_k=0.04 +GFTT_maxCorners=1000 +GFTT_minDistance=1 +GFTT_qualityLevel=0.01 +GFTT_useHarrisDetector=false +KAZE_extended=false +KAZE_nOctaveLayers=4 +KAZE_nOctaves=4 +KAZE_threshold=0.001 +KAZE_upright=false +LATCH_bytes=32 +LATCH_half_ssd_size=3 +LATCH_rotationInvariance=true +LUCID_blur_kernel=2 +LUCID_kernel=1 +MSER_areaThreshold=1.01 +MSER_delta=5 +MSER_edgeBlurSize=5 +MSER_maxArea=14400 +MSER_maxEvolution=200 +MSER_maxVariation=0.25 +MSER_minArea=60 +MSER_minDiversity=0.2 +MSER_minMargin=0.003 +ORB_WTA_K=2 +ORB_blurForDescriptor=false +ORB_edgeThreshold=31 +ORB_firstLevel=0 +ORB_gpu=false +ORB_nFeatures=500 +ORB_nLevels=8 +ORB_patchSize=31 +ORB_scaleFactor=1.2 +ORB_scoreType=0 +SIFT_contrastThreshold=0.04 +SIFT_edgeThreshold=10 +SIFT_nOctaveLayers=3 +SIFT_nfeatures=0 +SIFT_rootSIFT=false +SIFT_sigma=1.6 +SURF_extended=true +SURF_gpu=false +SURF_hessianThreshold=600 +SURF_keypointsRatio=0.01 +SURF_nOctaveLayers=2 +SURF_nOctaves=4 +SURF_upright=false +Star_lineThresholdBinarized=8 +Star_lineThresholdProjected=10 +Star_maxSize=45 +Star_responseThreshold=30 +Star_suppressNonmaxSize=5 +SuperPointTorch_NMS=true +SuperPointTorch_NMS_radius=4 +SuperPointTorch_cuda=false +SuperPointTorch_modelPath= +SuperPointTorch_threshold=0.2 + +[%General] +autoPauseOnDetection=false +autoScreenshotPath= +autoScroll=true +autoStartCamera=false +autoUpdateObjects=true +controlsShown=false +debug=false +imageFormats=*.png *.jpg *.bmp *.tiff *.ppm +invertedSearch=true +mirrorView=false +multiDetection=false +multiDetectionRadius=30 +nextObjID=9 +port=0 +sendNoObjDetectedEvents=false +threads=1 +videoFormats=*.avi *.m4v *.mp4 +vocabularyFixed=false +vocabularyIncremental=false +vocabularyUpdateMinWords=2000 + +[Homography] +allCornersVisible=false +confidence=0.995 +homographyComputed=true +ignoreWhenAllInliers=false +maxIterations=2000 +method="1:LMEDS;RANSAC;RHO" +minAngle=50 +minimumInliers=10 +opticalFlow=false +opticalFlowEps=0.01 +opticalFlowIterations=30 +opticalFlowMaxLevel=3 +opticalFlowWinSize=16 +ransacReprojThr=5 +rectBorderWidth=4 + +[NearestNeighbor] +1Strategy="1:Linear;KDTree;KMeans;Composite;Autotuned;Lsh;BruteForce" +2Distance_type="0:EUCLIDEAN_L2;MANHATTAN_L1;MINKOWSKI;MAX;HIST_INTERSECT;HELLINGER;CHI_SQUARE_CS;KULLBACK_LEIBLER_KL;HAMMING" +3nndrRatioUsed=true +4nndrRatio=0.8 +5minDistanceUsed=false +6minDistance=1.6 +7ConvertBinToFloat=false +7search_checks=32 +8search_eps=0 +9search_sorted=true +Autotuned_build_weight=0.01 +Autotuned_memory_weight=0 +Autotuned_sample_fraction=0.1 +Autotuned_target_precision=0.8 +BruteForce_gpu=false +Composite_branching=32 +Composite_cb_index=0.2 +Composite_centers_init="0:RANDOM;GONZALES;KMEANSPP" +Composite_iterations=11 +Composite_trees=4 +KDTree_trees=4 +KMeans_branching=32 +KMeans_cb_index=0.2 +KMeans_centers_init="0:RANDOM;GONZALES;KMEANSPP" +KMeans_iterations=11 +Lsh_key_size=20 +Lsh_multi_probe_level=2 +Lsh_table_number=12 +search_checks=32 +search_eps=0 +search_sorted=true diff --git a/rtabmap_demos/data/books/4.png b/rtabmap_demos/data/books/4.png new file mode 100644 index 00000000..5e5f9535 Binary files /dev/null and b/rtabmap_demos/data/books/4.png differ diff --git a/rtabmap_demos/data/books/5.png b/rtabmap_demos/data/books/5.png new file mode 100644 index 00000000..ffcc29c7 Binary files /dev/null and b/rtabmap_demos/data/books/5.png differ diff --git a/rtabmap_demos/data/books/6.png b/rtabmap_demos/data/books/6.png new file mode 100644 index 00000000..afe1e88b Binary files /dev/null and b/rtabmap_demos/data/books/6.png differ diff --git a/rtabmap_demos/data/books/7.png b/rtabmap_demos/data/books/7.png new file mode 100644 index 00000000..e01fc98f Binary files /dev/null and b/rtabmap_demos/data/books/7.png differ diff --git a/rtabmap_demos/data/books/8.png b/rtabmap_demos/data/books/8.png new file mode 100644 index 00000000..4c04a865 Binary files /dev/null and b/rtabmap_demos/data/books/8.png differ diff --git a/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py b/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py new file mode 100644 index 00000000..3432f02b --- /dev/null +++ b/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py @@ -0,0 +1,87 @@ + +# Requires installed https://github.com/chvmp/champ/tree/ros2 +# +# Example: +# 1) Launch simulator (gazebo, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos champ_sim_vslam.launch.py +# +# Note that the first time we launch gazebo, it may take a +# while to download all assets. You may need to restart the +# launch to make sure all nodes are started after the sim is ready. +# +# 2) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 launch champ_teleop teleop.launch.py +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + + sim_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_config'), 'launch', 'gazebo.launch.py'] + ) + + gz_pkg_share = FindPackageShare(package="champ_gazebo").find("champ_gazebo") + + champ_vslam = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'launch', 'champ', 'champ_vslam.launch.py'] + ) + + rviz = LaunchConfiguration('rviz').perform(context) + world = LaunchConfiguration('world').perform(context) + + return [ + TimerAction( + actions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(champ_vslam), + launch_arguments={ + 'use_sim_time': 'true', + 'rviz': rviz, + 'rtabmap_viz': LaunchConfiguration('rtabmap_viz'), + 'localization': LaunchConfiguration('localization'), + }.items() + )], period = 5.0), # Wait 5 sec to make sure simulator is ready + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(sim_launch_path), + launch_arguments={'rviz': 'false', + 'world': os.path.join(gz_pkg_share, f"worlds/{world}.world")}.items() + ), + ] + +def generate_launch_description(): + + return LaunchDescription([ + + DeclareLaunchArgument( + name='rviz', + default_value='true', + description='Run rviz' + ), + + DeclareLaunchArgument( + name='rtabmap_viz', + default_value='true', + description='Run rtabmap_viz' + ), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'world', default_value='playground', + choices=['outdoor', 'playground'], + description='Champ gazebo world.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/champ/champ_vslam.launch.py b/rtabmap_demos/launch/champ/champ_vslam.launch.py new file mode 100644 index 00000000..7b527168 --- /dev/null +++ b/rtabmap_demos/launch/champ/champ_vslam.launch.py @@ -0,0 +1,179 @@ + +# Similar to gazebo example on https://github.com/chvmp/champ/tree/ros2, we can do: +# +# Run the Gazebo environment: +# $ ros2 launch champ_config gazebo.launch.py +# +# Run Nav2's navigation and rtabmap: +# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true +# +# When a map is already created using command above, we can re-launch in localization-only mode with: +# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true localization:=true +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +def launch_setup(context, *args, **kwargs): + + localization = LaunchConfiguration('localization') + + navigation_launch_path = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py'] + ) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'champ_nav2_params.yaml'] + ) + + rviz_config_path = PathJoinSubstitution( + [FindPackageShare('champ_navigation'), 'rviz', 'navigation.rviz'] + ) + + use_sim_time = LaunchConfiguration("use_sim_time") + + # With the simulator, the imu is not published fast enough + # and have a huge delay, disabling imu usage from VO + use_imu = use_sim_time.perform(context) in ["false", "False"] + + vslam_params ={ + 'frame_id':'base_link', + 'guess_frame_id':'odom', + 'approx_sync': False, + 'use_sim_time':use_sim_time, + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'use_action_for_goal':True, + 'wait_imu_to_init': use_imu, + 'wait_for_transform': 0.5, + # RTAB-Map's parameters should be strings + 'Grid/DepthDecimation': '1', + 'Grid/RangeMax': '2', + 'GridGlobal/MinSize': '20', + 'Grid/MinClusterSize': '20', + 'Grid/MaxObstacleHeight': '2', + 'Odom/ResetCountdown': '2', # sim is very flaky + 'Kp/RoiRatios': '0.0 0.0 0.0 0.4' # ignore ground for loop closure detection (sim uses a very repetitive texture) + } + + vslam_remappings=[('imu', 'imu/data/filtered'), + ('odom', 'vo')] + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(navigation_launch_path), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_params_file + }.items() + ), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_path], + condition=IfCondition(LaunchConfiguration("rviz")), + parameters=[{'use_sim_time': use_sim_time}] + ), + + # compute imu orientation + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{ + 'use_mag':False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[ + ('imu/data_raw', 'imu/data'), + ('imu/data', 'imu/data/filtered') + ]), + + # VSLAM nodes: + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[vslam_params], + remappings=[('rgb/image', '/camera/image_raw'), + ('rgb/camera_info', '/camera/camera_info'), + ('depth/image', '/camera/depth/image_raw')]), + + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=[vslam_params, {'odom_frame_id': 'vo'}], + remappings=vslam_remappings, + arguments=["--ros-args", "--log-level", 'info']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[vslam_params], + remappings=vslam_remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[vslam_params, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=vslam_remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[vslam_params], + remappings=vslam_remappings), + + # Compute ground/obstacle clouds for nav2 voxel layers + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/depth/camera_info'), + ('cloud', '/camera/cloud')]), + + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[vslam_params], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ] + +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument( + name='use_sim_time', + default_value='false', + description='Enable use_sime_time to true' + ), + + DeclareLaunchArgument( + name='rviz', + default_value='false', + description='Run rviz' + ), + + DeclareLaunchArgument( + name='rtabmap_viz', + default_value='false', + description='Run rtabmap_viz' + ), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/find_object_demo.launch.py b/rtabmap_demos/launch/find_object_demo.launch.py new file mode 100644 index 00000000..54a63a78 --- /dev/null +++ b/rtabmap_demos/launch/find_object_demo.launch.py @@ -0,0 +1,129 @@ +# Requirements: +# find_object_2d package installed +# Download rosbag: +# * demo_find_object.db3: https://drive.google.com/file/d/1web54yQkxeGFr2UwOjKeoajGGDm0fZXT/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos find_object_demo.launch.py +# +# Rosbag: +# $ ros2 bag play demo_find_object.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans + 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP + 'Reg/Force3DoF': 'true', # 2D SLAM + } + + remappings=[ + ('rgb/image', '/camera/data_throttled_image'), + ('depth/image', '/camera/data_throttled_image_depth'), + ('rgb/camera_info', '/camera/data_throttled_camera_info'), + ('scan', '/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + config_find_object = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'find_object.ini' + ) + + data_find_object = os.path.join( + get_package_share_directory('rtabmap_demos'), 'data', 'books' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for find_object + Node( + package='image_transport', executable='republish', name='republish_rgb', output='screen', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', '/camera/data_throttled_image/compressed'), + ('out', '/camera/data_throttled_image')]), + Node( + package='image_transport', executable='republish', name='republish_depth', output='screen', + arguments=['compressedDepth', 'raw'], + remappings=[('in/compressedDepth', '/camera/data_throttled_image_depth/compressedDepth'), + ('out', '/camera/data_throttled_image_depth')]), + + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + + # Find-Object + Node( + package='find_object_2d', executable='find_object_2d', output='screen', + parameters=[{'gui': True, + 'subscribe_depth': True, + 'settings_path': config_find_object, + 'objects_path': data_find_object}], + remappings=[('rgb/image_rect_color', '/camera/data_throttled_image'), + ('depth_registered/image_raw', '/camera/data_throttled_image_depth'), + ('depth_registered/camera_info', '/camera/data_throttled_camera_info')]), + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py new file mode 100644 index 00000000..d8f8d341 --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py @@ -0,0 +1,104 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam2d.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py new file mode 100644 index 00000000..1c60eb8f --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py @@ -0,0 +1,104 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743): +# - ogre2 +# + ogre +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d_assemble.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('use_sim_time', 'true'), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py new file mode 100644 index 00000000..ed6c896a --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py @@ -0,0 +1,110 @@ +# +# Requirements: +# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins +# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml +# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro: +# 1.047 +# +# 320 +# 240 +# +# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743): +# - ogre2 +# + ogre +# +# Example with gazebo: +# 1) Launch simulator (husky, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py robot_ns:=a200_0000 +# +# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 3) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +import os + +ARGUMENTS = [ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), + DeclareLaunchArgument('robot_ns', default_value='a200_0000', + description='Robot namespace'), + DeclareLaunchArgument('use_camera', default_value='true', + description='Use camera for global loop closure / re-localization.'), +] + +def generate_launch_description(): + # Directories + pkg_clearpath_gz = get_package_share_directory( + 'clearpath_gz') + pkg_clearpath_viz = get_package_share_directory( + 'clearpath_viz') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + pkg_clearpath_nav2_demos = get_package_share_directory( + 'clearpath_nav2_demos') + + # Paths + sim_launch = PathJoinSubstitution( + [pkg_clearpath_gz, 'launch', 'simulation.launch.py']) + viz_launch = PathJoinSubstitution( + [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py']) + + sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([sim_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ] + ) + + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([viz_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('robot_ns')), + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('use_camera', LaunchConfiguration('use_camera')), + ('robot_ns', LaunchConfiguration('robot_ns')) + ] + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('setup_path', os.path.expanduser('~')+'/clearpath/'), + ('use_sim_time', 'true'), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) + ld.add_action(sim) + ld.add_action(viz) + ld.add_action(nav2) + return ld diff --git a/rtabmap_demos/launch/husky/husky_slam2d.launch.py b/rtabmap_demos/launch/husky/husky_slam2d.launch.py new file mode 100644 index 00000000..53bfcf52 --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam2d.launch.py @@ -0,0 +1,128 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam2d.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + robot_ns = LaunchConfiguration('robot_ns') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.7', # ignore laser scan points on the robot itself + 'RGBD/OptimizeMaxError':'2', + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/PointToPlaneMinComplexity':'0.04', # to be more robust to long corridors with low geometry + 'Icp/MaxTranslation': '1' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('scan', 'sensors/lidar2d_0/scan'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d.launch.py b/rtabmap_demos/launch/husky/husky_slam3d.launch.py new file mode 100644 index 00000000..bbadcb25 --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam3d.launch.py @@ -0,0 +1,146 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam3d.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + robot_ns = LaunchConfiguration('robot_ns') + use_camera = LaunchConfiguration('use_camera') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size + 'OdomF2M/ScanMaxSize': '10000' + } + + rtabmap_parameters={ + 'subscribe_rgb':False, + 'subscribe_depth':False, + 'subscribe_rgbd': use_camera, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/VoxelSize': '0.3', + 'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size + 'Icp/PointToPlaneGroundNormalsUp': '0.9', + 'Icp/RangeMin': '0.5', + 'Icp/MaxTranslation': '1' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('scan_cloud', 'sensors/lidar3d_0/points'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + DeclareLaunchArgument( + 'use_camera', default_value='true', + description='Use camera for global loop closure / re-localization.'), + + # Nodes to launch + Node( + condition=IfCondition(use_camera), + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py new file mode 100644 index 00000000..62b4116f --- /dev/null +++ b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py @@ -0,0 +1,134 @@ +# +# +# Example with gazebo: +# 1) Launch simulator (husky): +# $ ros2 launch clearpath_gz simulation.launch.py +# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec. +# +# 2) Launch rviz: +# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000 +# +# 3) Launch SLAM: +# $ ros2 launch rtabmap_demos husky_slam3d_assemble.launch.py use_sim_time:=true +# +# 4) Launch nav2" +# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true +# +# 4) Click on "Play" button on bottom-left of gazebo. +# +# 5) Move the robot: +# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar. +# a) By teleoperating: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + robot_ns = LaunchConfiguration('robot_ns') + + icp_odom_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size + 'OdomF2M/ScanMaxSize': '10000' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_depth':False, + 'subscribe_rgb':False, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + 'topic_queue_size': 30, + 'sync_queue_size': 30, + 'approx_sync': True, + 'qos': 1, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false', + 'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures + 'Rtabmap/DetectionRate': '0' # Rate is limited by the assembling time below (1 Hz) + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', # we are moving on a 2D flat floor + 'Mem/NotLinkedNodesKept':'false', + 'Icp/VoxelSize': '0.3', + 'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size + 'Icp/PointToPlaneGroundNormalsUp': '0.9', + 'Icp/RangeMin': '0.5', + 'Icp/MaxTranslation': '2' + } + + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('rgb/image', 'sensors/camera_0/color/image'), + ('rgb/camera_info', 'sensors/camera_0/color/camera_info'), + ('depth/image', 'sensors/camera_0/depth/image')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'robot_ns', default_value='a200_0000', + description='Robot namespace.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=robot_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + namespace=robot_ns, + parameters=[icp_odom_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')], + arguments=["--ros-args", "--log-level", 'warn']), + + #Assemble scans + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + namespace=robot_ns, + parameters=[{'assembling_time': 1.0, 'range_min': 0.5, 'fixed_frame_id': "", 'use_sim_time':use_sim_time, 'sync_queue_size': 30, 'topic_queue_size':30}], + remappings=remappings + [('cloud', 'sensors/lidar3d_0/points')]), + + # SLAM Mode: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'assembled_cloud')], + arguments=['-d']), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace=robot_ns, + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]), + ]) diff --git a/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py b/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py new file mode 100644 index 00000000..521105d1 --- /dev/null +++ b/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py @@ -0,0 +1,280 @@ +# +# Requirements: +# * Isaac simulator +# * isaac_ros_image_proc +# * isaac_ros_stereo_image_proc +# * nav2_bringup +# * isaac_ros_visual_slam (optional, for vo:=isaac) +# +# 1. Launch Isaac Simulator +# +# 2. Open Isaac Examples -> ROS2 -> Navigation -> Carter Navigation (or iw.hub Navigation, for more visual features) +# +# 3. Enable front stereo right camera: +# In the Stage tab, open World->Nova_Carter_ROS->front_hawk->right_camera_render_product, +# then under Property->Isaac Create Render Product Node->Inputs, check "Enabled". To make +# simulation faster, set height=600 and width=960. Do the same for the front stereo left camera. +# +# 4. Make sure that after you click on Play button in the simulator, you can see these topics: +# $ ros2 topic list +# /front_stereo_camera/left/camera_info +# /front_stereo_camera/left/image_raw +# /front_stereo_camera/left/image_raw/nitros_bridge +# /front_stereo_camera/right/camera_info +# /front_stereo_camera/right/image_raw +# /front_stereo_camera/right/image_raw/nitros_bridge +# /front_stereo_imu/imu +# +# 5. Launch the example: +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py +# +# 6. You should be able to send goals in RVIZ to move the robot, or use: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# === Advanced === +# With this launch file, we can also experiment with visual odometry with/without disparity computed on GPU. +# +# A. Use RTAB-Map's Visual Odometry: +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=true +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=false +# +# B. Use Isaac Visual Odometry: +# We should disable wheel odometry TF publishing in the simulator to make it work. To +# do so, in the Stage tab, open World->Nova_Carter_ROS->transform_tree_odometry->ros2_publish_raw_transform_tree, +# then under Property->ROS2Publish Raw Transform Tree Node->Inputs, change topicName from "tf" to "tf_odom_ignored". +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=true +# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=false +# +# + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def launch_setup(context, *args, **kwargs): + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + nav2_vo_params = PathJoinSubstitution( + [pkg_rtabmap_demos, 'params', 'isaac_vslam_nav2_params.yaml']) + nav2_params = PathJoinSubstitution( + [pkg_rtabmap_demos, 'params', 'isaac_nav2_params.yaml']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'isaac', 'isaac_vslam.launch.py']) + + vo = LaunchConfiguration('vo').perform(context) + image_width = int(LaunchConfiguration('image_width').perform(context)) + image_height = int(LaunchConfiguration('image_height').perform(context)) + + left_resize_node = ComposableNode( + name='left_resize_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ResizeNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image', 'left/image_raw'), + ('camera_info', 'left/camera_info'), + ('resize/image', 'left/image_resize'), + ('resize/camera_info', 'left/camera_info_resize') + ] + ) + + right_resize_node = ComposableNode( + name='right_resize_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ResizeNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image', 'right/image_raw'), + ('camera_info', 'right/camera_info'), + ('resize/image', 'right/image_resize'), + ('resize/camera_info', 'right/camera_info_resize') + ] + ) + + left_rectify_node = ComposableNode( + name='left_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image_raw', 'left/image_resize'), + ('camera_info', 'left/camera_info_resize'), + ('image_rect', 'left/image_rect'), + ('camera_info_rect', 'left/camera_info_rect') + ] + ) + + right_rectify_node = ComposableNode( + name='right_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'use_sim_time': True, + 'output_width': image_width, + 'output_height': image_height, + }], + namespace="front_stereo_camera", + remappings=[ + ('image_raw', 'right/image_resize'), + ('camera_info', 'right/camera_info_resize'), + ('image_rect', 'right/image_rect'), + ('camera_info_rect', 'right/camera_info_rect') + ] + ) + + disparity_node = ComposableNode( + name='disparity_node', + package='isaac_ros_stereo_image_proc', + plugin='nvidia::isaac_ros::stereo_image_proc::DisparityNode', + parameters=[{ + 'use_sim_time': True, + 'backends': 'CUDA', + 'max_disparity': 64.0 + }], + namespace="front_stereo_camera", + remappings=[ + ('left/camera_info', 'left/camera_info_rect'), + ('right/camera_info', 'right/camera_info_rect'), + ], + ) + + disparity_to_depth_node = ComposableNode( + name='disparity_to_depth_node', + package='isaac_ros_stereo_image_proc', + plugin='nvidia::isaac_ros::stereo_image_proc::DisparityToDepthNode', + parameters=[{ + 'use_sim_time': True, + }], + namespace="front_stereo_camera" + ) + + stereo_img_proc_container = ComposableNodeContainer( + name='stereo_img_proc_container', + package='rclcpp_components', + namespace="front_stereo_camera", + executable='component_container_mt', + composable_node_descriptions=[ + left_resize_node, + right_resize_node, + left_rectify_node, + right_rectify_node, + disparity_node, + disparity_to_depth_node + ], + output='screen', + arguments=['--ros-args', '--log-level', 'info', + '--log-level', 'color_format_convert:=info', + '--log-level', 'NitrosImage:=info', + '--log-level', 'NitrosNode:=info' + ], + ) + + nav2_args = [('use_sim_time', 'true')] + if vo == 'rtabmap': + # We need to change the base odom frame to vo + nav2_args.append(('params_file', nav2_vo_params)) + else: + # Use custom version with higher velocities + nav2_args.append(('params_file', nav2_params)) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=nav2_args + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ('stereo_camera_namespace', 'front_stereo_camera'), + ('enable_vo', str(vo == 'rtabmap')), + ('stereo', LaunchConfiguration('stereo')) + ] + ) + + # Add actions + actions = [rtabmap, nav2, rviz, stereo_img_proc_container] + + if vo == 'isaac': + isaac_visual_slam_node = ComposableNode( + name='visual_slam_node', + package='isaac_ros_visual_slam', + plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode', + remappings=[('visual_slam/image_0', 'front_stereo_camera/left/image_rect'), + ('visual_slam/camera_info_0', 'front_stereo_camera/left/camera_info_rect'), + ('visual_slam/image_1', 'front_stereo_camera/right/image_rect'), + ('visual_slam/camera_info_1', 'front_stereo_camera/right/camera_info_rect')], + parameters=[{ + 'use_sim_time': True, + 'enable_image_denoising': True, + 'enable_planar_mode': True, + 'rectified_images': True, + 'publish_map_to_odom_tf': False, + 'odom_frame': 'odom', + 'enable_slam_visualization': True, + 'enable_observations_view': True, + 'enable_landmarks_view': True}] + ) + + isaac_vslam_container = ComposableNodeContainer( + name='isaac_visual_slam_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[isaac_visual_slam_node], + output='screen', + ) + actions.append(isaac_vslam_container) + + return actions + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('vo', default_value='none', + choices=['none', 'rtabmap', 'isaac'], description='Enable visual odometry using one of the approach. None means only wheel odometry is used. If you set this to "isaac", make sure to disable odom -> base_link if it exists, because isaac will publish on same TF!'), + DeclareLaunchArgument('stereo', default_value='true', + choices=['true', 'false'], description='Use stereo images as input instead of left+depth images.'), + DeclareLaunchArgument('image_width', default_value='960', + description='Resize input images.'), + DeclareLaunchArgument('image_height', default_value='600', + description='Resize input images.'), + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py new file mode 100644 index 00000000..6ad55bd8 --- /dev/null +++ b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py @@ -0,0 +1,139 @@ + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + +def launch_setup(context, *args, **kwargs): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + localization_value = localization.perform(context) + localization_value = localization_value == 'True' or localization_value == 'true' + enable_vo = LaunchConfiguration('enable_vo') + enable_vo_value = enable_vo.perform(context) + enable_vo_value = enable_vo_value == 'True' or enable_vo_value == 'true' + stereo = LaunchConfiguration('stereo') + stereo_value = stereo.perform(context) + stereo_value = stereo_value == 'True' or stereo_value == 'true' + rtabmap_viz = LaunchConfiguration('rtabmap_viz') + stereo_ns = LaunchConfiguration('stereo_camera_namespace').perform(context) + + parameters={ + 'frame_id':'base_link', + 'use_sim_time': use_sim_time, + 'subscribe_rgbd': True, + 'subscribe_odom': enable_vo, + 'subscribe_odom_info': enable_vo, + 'approx_sync': False, + 'use_action_for_goal':True, + 'Reg/Force3DoF':'true', + 'Vis/MinDepth': '0.2', + 'GFTT/MinDistance': '5', + 'GFTT/QualityLevel': '0.00001', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.15', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.5', # All points over 0.5 meter are ignored + 'Grid/RangeMin':'0.2', # Ignore invalid points close to camera + 'Grid/NoiseFilteringMinNeighbors':'8', # Default stereo is quite noisy, enable noise filter + 'Grid/NoiseFilteringRadius':'0.1', # Default stereo is quite noisy, enable noise filter + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + if enable_vo_value: + parameters['guess_frame_id'] = 'odom' + else: + parameters['odom_frame_id'] = 'odom' + + arguments = [] + if localization_value: + parameters['Mem/IncrementalMemory'] = 'True' + parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + remappings=[('rgbd_image', '/'+stereo_ns+'/rgbd_image'), + ('map', '/map')] + vo_node_prefix = 'rgbd' + if stereo_value: + vo_node_prefix = 'stereo' + + return [ + # Sync image data together + Node( + condition=UnlessCondition(stereo), + package='rtabmap_sync', executable='rgbd_sync', output='screen', + namespace=stereo_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=[ + ('rgb/image', 'left/image_rect'), + ('rgb/camera_info', 'left/camera_info_rect'), + ('depth/image', 'depth')]), + + Node( + condition=IfCondition(stereo), + package='rtabmap_sync', executable='stereo_sync', output='screen', + namespace=stereo_ns, + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('left/camera_info', 'left/camera_info_rect'), + ('right/image_rect', 'right/image_rect'), + ('right/camera_info', 'right/camera_info_rect')]), + + Node( + condition=IfCondition(enable_vo), + package='rtabmap_odom', executable=vo_node_prefix+'_odometry', output='screen', + namespace='rtabmap', + parameters=[parameters, {'odom_frame_id': 'vo'}], + remappings=remappings), + + # VSLAM: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + namespace='rtabmap', + parameters=[parameters], + remappings=remappings, + arguments=arguments), + + # Visualization: + Node( + condition=IfCondition(rtabmap_viz), + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + namespace='rtabmap', + parameters=[parameters], + remappings=remappings), + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'enable_vo', default_value='false', + description='Enable RTAB-Map\'s visual odometry.'), + + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', + description='Launch rtabmap_viz for visualization.'), + + DeclareLaunchArgument( + 'stereo', default_value='false', + description='Use stereo images as input instead of left+depth images.'), + + DeclareLaunchArgument( + 'stereo_camera_namespace', default_value='front_stereo_camera', + description='Namespace of the stereo camera.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/multisession_mapping_demo.launch.py b/rtabmap_demos/launch/multisession_mapping_demo.launch.py new file mode 100644 index 00000000..b5841039 --- /dev/null +++ b/rtabmap_demos/launch/multisession_mapping_demo.launch.py @@ -0,0 +1,115 @@ +# Requirements: +# Download one or more rosbags: +# * map1.db3: https://drive.google.com/file/d/1XajzWm0u1Tk7m7x63ybcKVMXj80r5P6r/view?usp=drive_link +# * map2.db3: https://drive.google.com/file/d/1_FxEalE2O-DQKq2tRpLIpDn5Mbvu0jZc/view?usp=drive_link +# * map3.db3: https://drive.google.com/file/d/1dJzMOoRPA28gQZUIWCeAa08Qn4wG9oRw/view?usp=drive_link +# * map4.db3: https://drive.google.com/file/d/19Y6yye0ndIIwdhEWMwTdoiSy9WlKS44c/view?usp=drive_link +# * map5.db3: https://drive.google.com/file/d/1zCx4Q4SftPplQtW1xeG-W3OkbTxwd5GD/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ rm ~/.ros/rtabmap.db +# $ ros2 launch rtabmap_demos multisession_mapping_demo.launch.py +# +# Rosbag: +# $ ros2 bag play map1.db3 --clock +# when done, you can play the next bag(s): +# $ ros2 bag play map2.db3 --clock +# $ ros2 bag play map3.db3 --clock +# $ ros2 bag play map4.db3 --clock +# $ ros2 bag play map5.db3 --clock +# +# Refer to this paper for more info: https://arxiv.org/abs/2407.15305 +# +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'false', + 'RGBD/ProximityBySpace': 'false', # Referred paper did only global loop closure detection + 'RGBD/OptimizeFromGraphEnd': 'true', + 'Reg/Strategy': '1', + 'Icp/Iterations': '30', + 'Icp/VoxelSize': '0', + 'Vis/MinInliers': '12', + 'Vis/MaxDepth': '0', + 'RGBD/AngularUpdate': '0.01', + 'RGBD/LinearUpdate': '0.01', + 'Rtabmap/TimeThr': '700', + 'Mem/RehearsalSimilarity': '0.30', # Referred paper used 0.45 with SURF, here with SIFT, we will use 0.3 + 'Kp/TfIdfLikelihoodUsed': 'false', + 'Bayes/FullPredictionUpdate': 'true', + 'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries + 'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries + 'Kp/MaxFeatures': '400', + 'Reg/Force3DoF': 'true', + 'RGBD/OptimizeMaxError': '10', + 'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2) + 'Optimizer/Iterations': '100', + 'Kp/IncrementalFlann': 'false', # Referred paper didn't use incremental FLANN + 'Icp/MaxTranslation': '0.5', + } + + remappings=[ + ('rgb/image', '/data_throttled_image'), + ('depth/image', '/data_throttled_image_depth'), + ('rgb/camera_info', '/data_throttled_camera_info'), + ('scan', '/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'rgb_image_transport':'compressed', + 'depth_image_transport':'compressedDepth', + 'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM node: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_demos/launch/robot_mapping_demo.launch.py b/rtabmap_demos/launch/robot_mapping_demo.launch.py new file mode 100644 index 00000000..d6ed7451 --- /dev/null +++ b/rtabmap_demos/launch/robot_mapping_demo.launch.py @@ -0,0 +1,112 @@ +# Requirements: +# Download rosbag: +# * demo_mapping.db3: https://drive.google.com/file/d/1v9qJ2U7GlYhqBJr7OQHWbDSCfgiVaLWb/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play demo_mapping.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans + 'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM + 'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM + 'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together. + 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP + 'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure + 'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated + 'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map + 'Reg/Force3DoF': 'true', # 2D SLAM + 'Grid/FromDepth': 'false', # Create 2D occupancy grid from laser scan + 'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations + 'RGBD/LocalRadius': '5', # limit length of proximity detections + 'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure + 'Icp/PM': 'false', + 'Icp/PointToPlane': 'false', + 'Icp/MaxCorrespondenceDistance': '0.15', + 'Icp/VoxelSize': '0.05' + } + + remappings=[ + ('rgb/image', '/data_throttled_image'), + ('depth/image', '/data_throttled_image_depth'), + ('rgb/camera_info', '/data_throttled_camera_info'), + ('scan', '/jn0/base_scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + {'rgb_image_transport':'compressed', + 'depth_image_transport':'compressedDepth', + 'approx_sync_max_interval': 0.02}], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py new file mode 100644 index 00000000..41036389 --- /dev/null +++ b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py @@ -0,0 +1,155 @@ +# Requirements: +# Download one or both rosbags: +# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link +# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play stereo_outdoorA.db3 --clock +# when done, you can play the secon bag: +# $ ros2 bag play stereo_outdoorB.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node, SetParameter, SetRemap +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + pkg_stereo_image_proc = get_package_share_directory( + 'stereo_image_proc') + + # Paths + stereo_image_proc_launch = PathJoinSubstitution( + [pkg_stereo_image_proc, 'launch', 'stereo_image_proc.launch.py']) + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'subscribe_rgbd':True, + 'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs + 'map_negative_poses_ignored':True, + 'subscribe_odom_info': True, + # RTAB-Map's internal parameters should be strings + 'OdomF2M/MaxSize': '1000', + 'GFTT/MinDistance': '10', + 'GFTT/QualityLevel': '0.00001', + #'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + #'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + } + + remappings=[ + ('rgbd_image', '/stereo_camera/rgbd_image'), + ('odom', '/vo')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc + Node( + package='image_transport', executable='republish', name='republish_left', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'left/image_raw_throttle/compressed'), + ('out', 'left/image_raw')]), + Node( + package='image_transport', executable='republish', name='republish_right', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'right/image_raw_throttle/compressed'), + ('out', 'right/image_raw')]), + + # Run the ROS package stereo_image_proc for image rectification + GroupAction( + actions=[ + + SetRemap(src='camera_info',dst='camera_info_throttle'), + SetRemap(src='camera_info',dst='camera_info_throttle'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([stereo_image_proc_launch]), + launch_arguments=[ + ('left_namespace', 'stereo_camera/left'), + ('right_namespace', 'stereo_camera/right'), + ('disparity_range', '128'), + ] + ), + ] + ), + + # Synchronize stereo data together in a single topic + # Issue: stereo_img_proc doesn't produce color and + # grayscale images exactly the same (there is a small + # vertical shift with color), we should use grayscale for + # left and right images to get similar results than on ros1 noetic. + Node( + package='rtabmap_sync', executable='stereo_sync', output='screen', + namespace='stereo_camera', + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('right/image_rect', 'right/image_rect'), + ('left/camera_info', 'left/camera_info_throttle'), + ('right/camera_info', 'right/camera_info_throttle')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[parameters], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) + + + + + diff --git a/rtabmap_demos/launch/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py similarity index 57% rename from rtabmap_demos/launch/turtlebot3_rgbd.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index 93f3d97d..b9076449 100644 --- a/rtabmap_demos/launch/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -1,34 +1,15 @@ -# Requirements: -# Install Turtlebot3 packages -# Modify turtlebot3_waffle SDF: -# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf -# 2) Add -# -# camera_rgb_frame -# camera_rgb_optical_frame -# 0 0 0 -1.57079632679 0 -1.57079632679 -# -# 0 0 1 -# -# -# 3) Rename to -# 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 -# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: @@ -43,7 +24,6 @@ from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') parameters={ @@ -51,9 +31,13 @@ def generate_launch_description(): 'use_sim_time':use_sim_time, 'subscribe_depth':True, 'use_action_for_goal':True, - 'qos_image':qos, - 'qos_imu':qos, 'Reg/Force3DoF':'true', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -69,10 +53,6 @@ def generate_launch_description(): 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true'), - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), @@ -100,4 +80,22 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[parameters], remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py new file mode 100644 index 00000000..1ccacd65 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py @@ -0,0 +1,143 @@ +# Example: +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py +# +# SLAM: +# $ ros2 launch rtabmap_demos turtlebot3_rgbd_fake_scan.launch.py +# +# Navigation (install nav2_bringup package): +# $ ros2 launch nav2_bringup navigation_launch.py +# $ ros2 launch nav2_bringup rviz_launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'use_sim_time':use_sim_time, + 'subscribe_rgbd':True, + 'subscribe_scan_cloud':True, + 'use_action_for_goal':True, + 'scan_cloud_is_2d': True, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + + remappings=[ + ('rgb/image', '/camera/image_raw'), + ('rgb/camera_info', '/camera/camera_info'), + ('depth/image', '/camera/depth/image_raw'), + ('scan_cloud', 'assembled_cloud')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + remappings=remappings), + + # Convert middle row of depth pixels to a fake laser scan + Node( + package='depthimage_to_laserscan', executable='depthimage_to_laserscan_node', output='screen', + parameters=[{ + 'use_sim_time':use_sim_time, + 'range_max': 5.0 + }], + remappings=[ + ('depth', '/camera/depth/image_raw'), + ('depth_camera_info', '/camera/camera_info'), + ('scan', '/camera/scan') + ]), + + # Just to convert the fake laser scan to PointCloud2 + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{'use_sim_time':use_sim_time, + 'fixed_frame_id': 'camera_link'}], # use camera frame + remappings=[ + ('input_scan', '/camera/scan') + ]), + + # Assemble the fake laser scans using a circular buffer, then feed that cloud to rtabmap + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + parameters=[{'use_sim_time':use_sim_time, + 'max_clouds': 20, + 'voxel_size': 0.05, + 'wait_for_transform': 1.0, + 'linear_update': 0.3, + 'angular_update': 0.5, + 'circular_buffer': True, + 'frame_id': 'base_link'}], + remappings=[ + ('assembled_cloud', 'assembled_cloud'), + ('cloud', '/camera/scan/deskewed') + ]), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ]) diff --git a/rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py similarity index 56% rename from rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py index e1034ab7..733330fc 100644 --- a/rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py @@ -1,34 +1,15 @@ -# Requirements: -# Install Turtlebot3 packages -# Modify turtlebot3_waffle SDF: -# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf -# 2) Add -# -# camera_rgb_frame -# camera_rgb_optical_frame -# 0 0 0 -1.57079632679 0 -1.57079632679 -# -# 0 0 1 -# -# -# 3) Rename to -# 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 -# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_rgbd_sync.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_rgbd_scan.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: @@ -44,7 +25,6 @@ from launch_ros.actions import Node def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') localization = LaunchConfiguration('localization') parameters={ @@ -53,13 +33,17 @@ def generate_launch_description(): 'subscribe_rgbd':True, 'subscribe_scan':True, 'use_action_for_goal':True, - 'qos_scan':qos, - 'qos_image':qos, - 'qos_imu':qos, # RTAB-Map's parameters should be strings: 'Reg/Strategy':'1', 'Reg/Force3DoF':'true', 'RGBD/NeighborLinkRefining':'True', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -73,13 +57,9 @@ def generate_launch_description(): # Launch arguments DeclareLaunchArgument( - 'use_sim_time', default_value='true', + 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - + DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), @@ -87,7 +67,7 @@ def generate_launch_description(): # Nodes to launch Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', - parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}], + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], remappings=remappings), # SLAM Mode: @@ -111,4 +91,22 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=[parameters], remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/cloud'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), ]) diff --git a/rtabmap_demos/launch/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py similarity index 54% rename from rtabmap_demos/launch/turtlebot3_scan.launch.py rename to rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py index 721180a9..e027c7e0 100644 --- a/rtabmap_demos/launch/turtlebot3_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py @@ -1,36 +1,32 @@ -# Requirements: -# Install Turtlebot3 packages -# Note that we can edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot_waffle/model.sdf -# to increase min scan range from 0.12 to 0.2 to avoid having scans -# hitting the robot itself # Example: -# $ export TURTLEBOT3_MODEL=waffle -# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +# +# Bringup turtlebot3: +# $ export TURTLEBOT3_MODEL=waffle +# $ export LDS_MODEL=LDS-01 +# $ ros2 launch turtlebot3_bringup robot.launch.py # # SLAM: -# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true qos:=2 -# $ ros2 run topic_tools relay /rtabmap/map /map +# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py # # Navigation (install nav2_bringup package): -# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +# $ ros2 launch nav2_bringup navigation_launch.py # $ ros2 launch nav2_bringup rviz_launch.py # # Teleop: # $ ros2 run turtlebot3_teleop teleop_keyboard from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node -def generate_launch_description(): - +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') - qos = LaunchConfiguration('qos') - localization = LaunchConfiguration('localization') + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'True' or localization == 'true' + icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' parameters={ 'frame_id':'base_footprint', @@ -40,18 +36,51 @@ def generate_launch_description(): 'subscribe_scan':True, 'approx_sync':True, 'use_action_for_goal':True, - 'qos_scan':qos, - 'qos_imu':qos, 'Reg/Strategy':'1', 'Reg/Force3DoF':'true', 'RGBD/NeighborLinkRefining':'True', 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } - + arguments = [] + if localization: + parameters['Mem/IncrementalMemory'] = 'False' + parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + remappings=[ ('scan', '/scan')] + if icp_odometry: + remappings.append(('odom', 'icp_odom')) + + return [ + # Nodes to launch + + # ICP odometry (optional) + Node( + condition=IfCondition(LaunchConfiguration('icp_odometry')), + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[parameters, + {'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom'}], + remappings=remappings), + + # SLAM: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=arguments), + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + ] + +def generate_launch_description(): return LaunchDescription([ # Launch arguments @@ -59,35 +88,13 @@ def generate_launch_description(): 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true'), - DeclareLaunchArgument( - 'qos', default_value='2', - description='QoS used for input sensor topics'), - DeclareLaunchArgument( 'localization', default_value='false', description='Launch in localization mode.'), - - # Nodes to launch - # SLAM mode: - Node( - condition=UnlessCondition(localization), - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[parameters], - remappings=remappings, - arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) - - # Localization mode: - Node( - condition=IfCondition(localization), - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[parameters, - {'Mem/IncrementalMemory':'False', - 'Mem/InitWMWithAllNodes':'True'}], - remappings=remappings), + DeclareLaunchArgument( + 'icp_odometry', default_value='false', + description='Launch ICP odometry on top of wheel odometry.'), - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], - remappings=remappings), + OpaqueFunction(function=launch_setup) ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py new file mode 100644 index 00000000..ad14ca21 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py @@ -0,0 +1,117 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py new file mode 100644 index 00000000..314ad3eb --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py @@ -0,0 +1,117 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py new file mode 100644 index 00000000..5a826cb6 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py @@ -0,0 +1,119 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) Add +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 6) Change image width/height from 1920x1080 to 640x480 +# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans +# hitting the robot itself +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world = LaunchConfiguration('world').perform(context) + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) + + # Paths + gazebo_launch = PathJoinSubstitution( + [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py']) + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py']) + + # Includes + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gazebo + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='house', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py new file mode 100644 index 00000000..284b5b68 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py @@ -0,0 +1,161 @@ +# Requirements: +# Install Turtlebot3 packages +# Modify turtlebot3_waffle SDF: +# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +# 2) We can increase min scan range from 0.12 to 0.2 to avoid having scans +# hitting the robot itself +# +# Example: +# $ ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py +# +# Teleop: +# $ ros2 run turtlebot3_teleop teleop_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +import os + +def launch_setup(context, *args, **kwargs): + if not 'TURTLEBOT3_MODEL' in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + world_name = LaunchConfiguration('world').perform(context) + + icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' + if icp_odometry: + # modified nav2 params to use icp_odom instead odom frame + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] + ) + else: + # original nav2 params + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] + ) + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py']) + + # To use ICP odometry, we should increase clock rate of gazebo, we copied content of + # turtlebot3_gazebo/launch/turtlebot3_world.launch here + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + f'turtlebot3_{world_name}.world' + ) + + import tempfile + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: + clock_override_file.write("---\n"+ + "gazebo:\n"+ + " ros__parameters:\n"+ + " publish_rate: 100.0") + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') + ), + launch_arguments={ + 'world': world, + 'params_file': clock_override_file.name}.items() + ) + + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') + ) + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': 'true'}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': LaunchConfiguration('x_pose'), + 'y_pose': LaunchConfiguration('y_pose') + }.items() + ) + + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', nav2_params_file) + ] + ) + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ) + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true') + ] + ) + return [ + # Nodes to launch + nav2, + rviz, + rtabmap, + gzserver_cmd, + gzclient_cmd, + robot_state_publisher_cmd, + spawn_turtlebot_cmd + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='world', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 gazebo world.'), + + DeclareLaunchArgument( + 'icp_odometry', default_value='false', + description='Launch ICP odometry on top of wheel odometry.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial position of the robot in the simulator.'), + + DeclareLaunchArgument( + 'y_pose', default_value='0.5', + description='Initial position of the robot in the simulator.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py new file mode 100644 index 00000000..965919df --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py @@ -0,0 +1,79 @@ +# +# 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_sim_demo.launch.py +# +# 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. +# +# 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 +# + +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', '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')), + ('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 diff --git a/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py new file mode 100644 index 00000000..85558ca8 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py @@ -0,0 +1,119 @@ +# +# 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 +# +# 2) Launch SLAM: +# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true +# OR +# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true +# +# 3) Click on "Play" button on bottom-left of gazebo. +# +# 4) Click on double points ".." button on top-right next to power button to undock. +# +# 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 +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + rtabmap_viz = LaunchConfiguration('rtabmap_viz') + + icp_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom' + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'odom_sensor_sync': True, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false' + } + + # 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', + 'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry + } + + 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( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', choices=['true', 'false'], + description='Launch rtabmap_viz for visualization.'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}], + 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( + condition=IfCondition(rtabmap_viz), + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ]) diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 18865042..cc7559f7 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -2,7 +2,7 @@ rtabmap_demos - 0.21.5 + 0.22.0 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/params/champ_nav2_params.yaml b/rtabmap_demos/params/champ_nav2_params.yaml new file mode 100644 index 00000000..6006036d --- /dev/null +++ b/rtabmap_demos/params/champ_nav2_params.yaml @@ -0,0 +1,288 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 5.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 2.0 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/isaac_nav2_params.yaml b/rtabmap_demos/params/isaac_nav2_params.yaml new file mode 100644 index 00000000..1376952e --- /dev/null +++ b/rtabmap_demos/params/isaac_nav2_params.yaml @@ -0,0 +1,295 @@ +# Isaac example: We increased max velocities. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /chassis/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.0 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.0 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [2.0, 0.0, 2.0] + min_velocity: [-2.0, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/chassis/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/rtabmap_demos/params/isaac_vslam_nav2_params.yaml b/rtabmap_demos/params/isaac_vslam_nav2_params.yaml new file mode 100644 index 00000000..0210b63e --- /dev/null +++ b/rtabmap_demos/params/isaac_vslam_nav2_params.yaml @@ -0,0 +1,295 @@ +# Isaac example: we changed the main odom_frame_id from "odom" to "vo" frame. We increased max velocities. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /chassis/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.0 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.0 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: vo + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: vo + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [2.0, 0.0, 2.0] + min_velocity: [-2.0, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/chassis/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml new file mode 100644 index 00000000..838c80b3 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml @@ -0,0 +1,287 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml new file mode 100644 index 00000000..43dce5ba --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml new file mode 100644 index 00000000..9c33bdb2 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml @@ -0,0 +1,295 @@ +# Modified to use icp_odom frame +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: icp_odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: icp_odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_examples/CMakeLists.txt b/rtabmap_examples/CMakeLists.txt index a8020127..07ea312c 100644 --- a/rtabmap_examples/CMakeLists.txt +++ b/rtabmap_examples/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_examples) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/rtabmap_examples/launch/config/euroc_left.yaml b/rtabmap_examples/config/euroc_left.yaml similarity index 100% rename from rtabmap_examples/launch/config/euroc_left.yaml rename to rtabmap_examples/config/euroc_left.yaml diff --git a/rtabmap_examples/launch/config/euroc_right.yaml b/rtabmap_examples/config/euroc_right.yaml similarity index 100% rename from rtabmap_examples/launch/config/euroc_right.yaml rename to rtabmap_examples/config/euroc_right.yaml diff --git a/rtabmap_examples/launch/config/slam_D405x2_config.rviz b/rtabmap_examples/config/slam_D405x2_config.rviz similarity index 100% rename from rtabmap_examples/launch/config/slam_D405x2_config.rviz rename to rtabmap_examples/config/slam_D405x2_config.rviz diff --git a/rtabmap_examples/launch/config/slam_D405x3_config.rviz b/rtabmap_examples/config/slam_D405x3_config.rviz similarity index 100% rename from rtabmap_examples/launch/config/slam_D405x3_config.rviz rename to rtabmap_examples/config/slam_D405x3_config.rviz diff --git a/rtabmap_examples/launch/depthai.launch.py b/rtabmap_examples/launch/depthai.launch.py new file mode 100644 index 00000000..8918bba0 --- /dev/null +++ b/rtabmap_examples/launch/depthai.launch.py @@ -0,0 +1,72 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros) +# Example: +# $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters=[{'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'depth_aligned': 'false', + 'enableRviz': 'false', + 'monoResolution': '400p'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=parameters, + remappings=[('rgb/image', '/right/image_rect'), + ('rgb/camera_info', '/right/camera_info'), + ('depth/image', '/stereo/depth')]), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ]) diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index 01112ed9..8c9f31ca 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): # Image rectification and publishing synchronized camera_info Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_left.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_left.yaml']}], remappings=[ ('image', '/cam0/image_raw'), ('camera_info', 'left/camera_info')], @@ -96,7 +96,7 @@ def generate_launch_description(): Node( package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen', - parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_right.yaml']}], + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_right.yaml']}], remappings=[ ('image', '/cam1/image_raw'), ('camera_info', 'right/camera_info')], diff --git a/rtabmap_examples/launch/k4a.launch.py b/rtabmap_examples/launch/k4a.launch.py index 346bf2e0..d09693a6 100644 --- a/rtabmap_examples/launch/k4a.launch.py +++ b/rtabmap_examples/launch/k4a.launch.py @@ -1,21 +1,20 @@ # Requirements: # A Kinect for Azure # Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble) +# To install Kinect SDK on Ubuntu 22.04, see https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1790#issuecomment-1531626651 +# udev rules: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/5f79890933e1c81e325633152b2f2799df825b8b/docs/usage.md#linux-device-setup # Install imu_filter_madgwick ros2 package # Example: # $ ros2 launch rtabmap_examples k4a.launch.py from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): parameters=[{ 'frame_id':'camera_base', 'subscribe_rgbd':True, - 'subscribe_odom_info':True, - 'qos':1}] + 'subscribe_odom_info':True}] remappings=[ ('imu', '/imu/data'), @@ -32,11 +31,9 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=[{ 'frame_id':'camera_base', - 'subscribe_odom_info':True, 'approx_sync':True, 'approx_sync_max_interval':0.01, 'wait_imu_to_init':True, - 'qos':1, 'queue_size':30, 'keep_color':True, # Color image needs to be rectified, diff --git a/rtabmap_examples/launch/kinect_xbox_360.launch.py b/rtabmap_examples/launch/kinect_xbox_360.launch.py index 2968e665..2b20d898 100644 --- a/rtabmap_examples/launch/kinect_xbox_360.launch.py +++ b/rtabmap_examples/launch/kinect_xbox_360.launch.py @@ -12,8 +12,7 @@ def generate_launch_description(): 'frame_id':'camera_link', 'subscribe_depth':True, 'subscribe_odom_info':True, - 'approx_sync':True, - 'qos':1}] + 'approx_sync':True}] remappings=[ ('rgb/image', '/kinect/rgb/image_raw'), diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py new file mode 100644 index 00000000..6ddee09a --- /dev/null +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -0,0 +1,253 @@ +# Description: +# In this example, we keep only minimal data to do LiDAR SLAM. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# If an IMU is used, make sure TF between lidar/base frame and imu is +# already calibrated. In this example, we assume the imu topic has +# already the orientation estimated, if not, you can use +# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false) +# and set imu_topic to output topic of the filter. +# +# If a camera is used, make sure TF between lidar/base frame and camera is +# already calibrated. To provide image data to this example, you should use +# rtabmap_sync's rgbd_sync or stereo_sync node. +# +# Launch the example by adjusting the lidar topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context: LaunchContext, *args, **kwargs): + + frame_id = LaunchConfiguration('frame_id') + + imu_topic = LaunchConfiguration('imu_topic') + imu_used = imu_topic.perform(context) != '' + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_topic_value + "/deskewed" + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == 'True' + + deskewing = LaunchConfiguration('deskewing').perform(context) + deskewing = deskewing == 'true' or deskewing == 'True' + + deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context) + deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True' + + fixed_frame_from_imu = False + fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) + if not fixed_frame_id and imu_used: + fixed_frame_from_imu = True + fixed_frame_id = frame_id.perform(context) + "_stabilized" + + if not fixed_frame_id or not deskewing: + lidar_topic_deskewed = lidar_topic + + # Rule of thumb: + max_correspondence_distance = voxel_size_value * 10.0 + + shared_parameters = { + 'use_sim_time': use_sim_time, + 'frame_id': frame_id, + 'qos': LaunchConfiguration('qos'), + 'approx_sync': rgbd_image_used, + 'wait_for_transform': 0.2, + # RTAB-Map's internal parameters are strings: + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/VoxelSize': str(voxel_size_value), + 'Icp/Epsilon': '0.001', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance), + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + } + + icp_odometry_parameters = { + 'expected_update_rate': LaunchConfiguration('expected_update_rate'), + 'deskewing': not fixed_frame_id and deskewing, # If fixed_frame_id is set, we do deskewing externally below + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': fixed_frame_id, + 'deskewing_slerp': deskewing_slerp, + # RTAB-Map's internal parameters are strings: + 'Odom/ScanKeyFrameThr': '0.4', + 'OdomF2M/ScanSubtractRadius': str(voxel_size_value), + 'OdomF2M/ScanMaxSize': '15000', + 'OdomF2M/BundleAdjustment': 'false', + 'Icp/CorrespondenceRatio': '0.01' + } + if imu_used: + icp_odometry_parameters['wait_imu_to_init'] = True + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + 'map_frame_id': 'new_map', + 'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps. + # RTAB-Map's internal parameters are strings: + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'RGBD/CreateOccupancyGrid': 'false', + 'Mem/NotLinkedNodesKept': 'false', + 'Mem/STMSize': '30', + 'Reg/Strategy': '1', + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) + } + + arguments = [] + if localization: + rtabmap_parameters['Mem/IncrementalMemory'] = 'False' + rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + remappings = [('odom', 'icp_odom')] + if imu_used: + remappings.append(('imu', LaunchConfiguration('imu_topic'))) + else: + remappings.append(('imu', 'imu_not_used')) + if rgbd_image_used: + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) + + nodes = [ + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, + {'subscribe_rgbd': rgbd_image_used, + 'rgbd_cameras': rgbd_cameras}], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)], + arguments=arguments), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) + ] + + if fixed_frame_from_imu: + # Create a stabilized base frame based on imu for lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='imu_to_tf', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'base_frame_id': frame_id, + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', imu_topic)])) + + if fixed_frame_id and deskewing: + # Lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2, + 'slerp': deskewing_slerp}], + remappings=[ + ('input_cloud', lidar_topic) + ]) + ) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'deskewing', default_value='true', + description='Enable lidar deskewing.'), + + DeclareLaunchArgument( + 'frame_id', default_value='velodyne', + description='Base frame of the robot.'), + + DeclareLaunchArgument( + 'fixed_frame_id', default_value='', + description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='', + description='IMU topic (ignored if empty).'), + + DeclareLaunchArgument( + 'rgbd_image_topic', default_value='', + description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'), + + DeclareLaunchArgument( + 'rgbd_images_topic', default_value='', + description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'), + + DeclareLaunchArgument( + 'expected_update_rate', default_value='15.0', + description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'min_loop_closure_overlap', default_value='0.2', + description='Minimum scan overlap pourcentage to accept a loop closure.'), + + DeclareLaunchArgument( + 'deskewing_slerp', default_value='true', + description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py new file mode 100644 index 00000000..9c217640 --- /dev/null +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -0,0 +1,273 @@ +# Description: +# In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated. +# In this example, we assume the imu topic has +# already the orientation estimated, if not, you can launch +# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false) +# and set imu_topic to output topic of the filter. +# +# If a camera is used, make sure TF between lidar/base frame and camera is +# already calibrated. To provide image data to this example, you should use +# rtabmap_sync's rgbd_sync or stereo_sync node. +# +# Launch the example by adjusting the lidar topic, imu topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context: LaunchContext, *args, **kwargs): + + frame_id = LaunchConfiguration('frame_id') + + external_odom_frame_id = LaunchConfiguration('external_odom_frame_id').perform(context) + + fixed_frame_from_imu = False + fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) + if not fixed_frame_id: + if external_odom_frame_id: + fixed_frame_id = external_odom_frame_id + else: + fixed_frame_from_imu = True + fixed_frame_id = frame_id.perform(context) + "_stabilized" + + imu_topic = LaunchConfiguration('imu_topic') + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_topic_value + "/deskewed" + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == 'True' + + deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context) + deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True' + + # Rule of thumb: + max_correspondence_distance = voxel_size_value * 10.0 + + shared_parameters = { + 'use_sim_time': use_sim_time, + 'frame_id': frame_id, + 'qos': LaunchConfiguration('qos'), + 'approx_sync': rgbd_image_used, + 'wait_for_transform': 0.2, + # RTAB-Map's internal parameters are strings: + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/VoxelSize': str(voxel_size_value), + 'Icp/Epsilon': '0.001', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance), + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + } + + icp_odometry_parameters = { + 'expected_update_rate': LaunchConfiguration('expected_update_rate'), + 'wait_imu_to_init': True, + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': fixed_frame_id, + # RTAB-Map's internal parameters are strings: + 'Odom/ScanKeyFrameThr': '0.4', + 'OdomF2M/ScanSubtractRadius': str(voxel_size_value), + 'OdomF2M/ScanMaxSize': '15000', + 'OdomF2M/BundleAdjustment': 'false', + 'Icp/CorrespondenceRatio': '0.01' + } + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': not external_odom_frame_id, + 'subscribe_scan_cloud': True, + 'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""), + 'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps. + # RTAB-Map's internal parameters are strings: + 'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s) + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'RGBD/CreateOccupancyGrid': 'false', + 'Mem/NotLinkedNodesKept': 'false', + 'Mem/STMSize': '30', + 'Reg/Strategy': '1', + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) + } + + remappings = [('imu', imu_topic), + ('odom', 'icp_odom')] + if rgbd_image_used: + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) + + arguments = [] + if localization: + rtabmap_parameters['Mem/IncrementalMemory'] = 'False' + rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + if external_odom_frame_id: + viz_topic = lidar_topic_deskewed + else: + viz_topic = 'odom_filtered_input_scan' + + nodes = [ + # Lidar deskewing + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2, + 'slerp': deskewing_slerp}], + remappings=[ + ('input_cloud', lidar_topic) + ]), + + # Assemble deskewed scans based on icp odometry + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'assembling_time': LaunchConfiguration('assembling_time'), + 'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom" + remappings=[('cloud', lidar_topic_deskewed), + ('odom', 'icp_odom')]), + + # Update the map + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, + {'subscribe_rgbd': rgbd_image_used, + 'rgbd_cameras': rgbd_cameras, + 'topic_queue_size': 40, + 'sync_queue_size': 40,}], + remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))], + arguments=arguments), + + # Just for visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', viz_topic)]) + ] + + if not external_odom_frame_id: + # Lidar odometry + nodes.append( + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)])) + + if fixed_frame_from_imu: + # Create a stabilized base frame based on imu for lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='imu_to_tf', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'base_frame_id': frame_id, + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', imu_topic)])) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'frame_id', default_value='velodyne', + description='Base frame of the robot.'), + + DeclareLaunchArgument( + 'fixed_frame_id', default_value='', + description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'), + + DeclareLaunchArgument( + 'external_odom_frame_id', default_value='', + description='Provide external odometry with TF, disabling icp_odometry.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='/imu/data', + description='Name of an IMU topic.'), + + DeclareLaunchArgument( + 'gps_topic', default_value='/gps/fix', + description='Name of a GPS topic.'), + + DeclareLaunchArgument( + 'rgbd_image_topic', default_value='', + description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'), + + DeclareLaunchArgument( + 'rgbd_images_topic', default_value='', + description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'min_loop_closure_overlap', default_value='0.2', + description='Minimum scan overlap pourcentage to accept a loop closure.'), + + DeclareLaunchArgument( + 'expected_update_rate', default_value='15.0', + description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'), + + DeclareLaunchArgument( + 'assembling_time', default_value='1.0', + description='How much time (sec) we assemble lidar scans before sending them to mapping node.'), + + DeclareLaunchArgument( + 'deskewing_slerp', default_value='true', + description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py new file mode 100644 index 00000000..23da663f --- /dev/null +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -0,0 +1,305 @@ +# Description: +# In this example, we will record ALL lidar scans from 2 lidars. An IMU or low latency odometry is required for this example. +# +# Example: +# Launch your lidar sensors +# In this example, we assume the lidar topics have a frame_id linked to same parent (e.g., base_link) and +# the extrinsics are known (URDF) and/or already calibrated. +# +# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated. +# In this example, we assume the imu topic has +# already the orientation estimated, if not, you can launch +# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false) +# and set imu_topic to output topic of the filter. +# +# If a camera is used, make sure TF between lidar/base frame and camera is +# already calibrated. To provide image data to this example, you should use +# rtabmap_sync's rgbd_sync or stereo_sync node. +# +# Launch the example by adjusting the lidar topics, imu topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar1_topic:=/lidar1/velodyne_points lidar2_topic:=/lidar1/velodyne_points imu_topic:=/imu/data frame_id:=base_link + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context: LaunchContext, *args, **kwargs): + + frame_id = LaunchConfiguration('frame_id') + + external_odom_frame_id = LaunchConfiguration('external_odom_frame_id').perform(context) + + fixed_frame_from_imu = False + fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) + if not fixed_frame_id: + if external_odom_frame_id: + fixed_frame_id = external_odom_frame_id + else: + fixed_frame_from_imu = True + fixed_frame_id = frame_id.perform(context) + "_stabilized" + + imu_topic = LaunchConfiguration('imu_topic') + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 + + lidar1_topic = LaunchConfiguration('lidar1_topic') + lidar1_topic_value = lidar1_topic.perform(context) + lidar1_topic_deskewed = lidar1_topic_value + "/deskewed" + + lidar2_topic = LaunchConfiguration('lidar2_topic') + lidar2_topic_value = lidar2_topic.perform(context) + lidar2_topic_deskewed = lidar2_topic_value + "/deskewed" + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == 'True' + + deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context) + deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True' + + # Rule of thumb: + max_correspondence_distance = voxel_size_value * 10.0 + + shared_parameters = { + 'use_sim_time': use_sim_time, + 'frame_id': frame_id, + 'qos': LaunchConfiguration('qos'), + 'approx_sync': rgbd_image_used, + 'wait_for_transform': 0.2, + # RTAB-Map's internal parameters are strings: + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/VoxelSize': str(voxel_size_value), + 'Icp/Epsilon': '0.001', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance), + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + } + + icp_odometry_parameters = { + 'expected_update_rate': LaunchConfiguration('expected_update_rate'), + 'wait_imu_to_init': True, + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': fixed_frame_id, + # RTAB-Map's internal parameters are strings: + 'Odom/ScanKeyFrameThr': '0.4', + 'OdomF2M/ScanSubtractRadius': str(voxel_size_value), + 'OdomF2M/ScanMaxSize': '15000', + 'OdomF2M/BundleAdjustment': 'false', + 'Icp/CorrespondenceRatio': '0.01' + } + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': not external_odom_frame_id, + 'subscribe_scan_cloud': True, + 'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""), + 'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps. + # RTAB-Map's internal parameters are strings: + 'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s) + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'RGBD/CreateOccupancyGrid': 'false', + 'Mem/NotLinkedNodesKept': 'false', + 'Mem/STMSize': '30', + 'Reg/Strategy': '1', + 'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context)) + } + + remappings = [('imu', imu_topic), + ('odom', 'icp_odom')] + if rgbd_image_used: + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) + + arguments = [] + if localization: + rtabmap_parameters['Mem/IncrementalMemory'] = 'False' + rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + if external_odom_frame_id: + viz_topic = "combined_cloud" + else: + viz_topic = 'odom_filtered_input_scan' + + nodes = [ + # Lidar1 deskewing + Node( + package='rtabmap_util', executable='lidar_deskewing', name="lidar1_deskewing", output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2, + 'slerp': deskewing_slerp}], + remappings=[ + ('input_cloud', lidar1_topic) + ]), + + # Lidar2 deskewing + Node( + package='rtabmap_util', executable='lidar_deskewing', name="lidar2_deskewing", output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2, + 'slerp': deskewing_slerp}], + remappings=[ + ('input_cloud', lidar2_topic) + ]), + + # Combine the two lidars in single point cloud + Node( + package='rtabmap_util', executable='point_cloud_aggregator', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'approx_sync': True, + 'fixed_frame_id': fixed_frame_id, + 'count': 2}], + remappings=[ + ('cloud1', lidar1_topic_deskewed), + ('cloud2', lidar2_topic_deskewed)]), + + # Assemble combined deskewed scans based on icp odometry + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'assembling_time': LaunchConfiguration('assembling_time'), + 'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom" + remappings=[('cloud', "combined_cloud"), + ('odom', 'icp_odom')]), + + # Update the map + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, + {'subscribe_rgbd': rgbd_image_used, + 'rgbd_cameras': rgbd_cameras, + 'topic_queue_size': 40, + 'sync_queue_size': 40,}], + remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))], + arguments=arguments), + + # Just for visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', viz_topic)]) + ] + + if not external_odom_frame_id: + # Lidar odometry + nodes.append( + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', "combined_cloud")])) + + if fixed_frame_from_imu: + # Create a stabilized base frame based on imu for lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='imu_to_tf', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'base_frame_id': frame_id, + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', imu_topic)])) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'frame_id', default_value='velodyne', + description='Base frame of the robot.'), + + DeclareLaunchArgument( + 'fixed_frame_id', default_value='', + description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'), + + DeclareLaunchArgument( + 'external_odom_frame_id', default_value='', + description='Provide external odometry with TF, disabling icp_odometry.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar1_topic', default_value='/lidar1/velodyne_points', + description='Name of the lidar1\'s PointCloud2 topic.'), + + DeclareLaunchArgument( + 'lidar2_topic', default_value='/lidar2/velodyne_points', + description='Name of the lidar2\'s PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='/imu/data', + description='Name of an IMU topic.'), + + DeclareLaunchArgument( + 'gps_topic', default_value='/gps/fix', + description='Name of a GPS topic.'), + + DeclareLaunchArgument( + 'rgbd_image_topic', default_value='', + description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'), + + DeclareLaunchArgument( + 'rgbd_images_topic', default_value='', + description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'min_loop_closure_overlap', default_value='0.2', + description='Minimum scan overlap pourcentage to accept a loop closure.'), + + DeclareLaunchArgument( + 'expected_update_rate', default_value='15.0', + description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'), + + DeclareLaunchArgument( + 'assembling_time', default_value='1.0', + description='How much time (sec) we assemble lidar scans before sending them to mapping node.'), + + DeclareLaunchArgument( + 'deskewing_slerp', default_value='true', + description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/realsense_d400.launch.py b/rtabmap_examples/launch/realsense_d400.launch.py index 79c1ffac..d2e47aff 100644 --- a/rtabmap_examples/launch/realsense_d400.launch.py +++ b/rtabmap_examples/launch/realsense_d400.launch.py @@ -2,16 +2,16 @@ # A realsense D400 series # Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238) # Example: -# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true -# # $ ros2 launch rtabmap_examples realsense_d400.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false + +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -27,7 +27,18 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'align_depth.enable': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index 4839874b..ae4e2edb 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -2,14 +2,17 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true -# # $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch_ros.actions import Node, SetParameter +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node def generate_launch_description(): parameters=[{ @@ -23,11 +26,32 @@ def generate_launch_description(): ('imu', '/imu/data'), ('rgb/image', '/camera/color/image_raw'), ('rgb/camera_info', '/camera/color/camera_info'), - ('depth/image', '/camera/realigned_depth_to_color/image_raw')] + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] return LaunchDescription([ - # Nodes to launch + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'align_depth.enable': 'true', + 'enable_sync': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, @@ -43,26 +67,7 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=parameters, remappings=remappings), - - # Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564 - # Generate point cloud from not aligned depth - Node( - package='rtabmap_util', executable='point_cloud_xyz', output='screen', - parameters=[{'approx_sync':False}], - remappings=[('depth/image', '/camera/depth/image_rect_raw'), - ('depth/camera_info', '/camera/depth/camera_info'), - ('cloud', '/camera/cloud_from_depth')]), - - # Generate aligned depth to color camera from the point cloud above - Node( - package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen', - parameters=[{ 'decimation':2, - 'fixed_frame_id':'camera_link', - 'fill_holes_size':1}], - remappings=[('camera_info', '/camera/color/camera_info'), - ('cloud', '/camera/cloud_from_depth'), - ('image_raw', '/camera/realigned_depth_to_color/image_raw')]), - + # Compute quaternion of the IMU Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', @@ -70,9 +75,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index d4a2116f..c5284b2c 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -2,15 +2,18 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node def generate_launch_description(): parameters=[{ @@ -28,7 +31,28 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, @@ -52,9 +76,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index 43f93a61..ccf865d3 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -2,15 +2,18 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node def generate_launch_description(): parameters=[{ @@ -28,7 +31,28 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', parameters=parameters, @@ -52,9 +76,4 @@ def generate_launch_description(): 'world_frame':'enu', 'publish_tf':False}], remappings=[('imu/data_raw', '/camera/imu')]), - - # The IMU frame is missing in TF tree, add it: - Node( - package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), ]) diff --git a/rtabmap_examples/launch/rgbdslam_datasets.launch.py b/rtabmap_examples/launch/rgbdslam_datasets.launch.py index 9aab2edd..92e8e030 100644 --- a/rtabmap_examples/launch/rgbdslam_datasets.launch.py +++ b/rtabmap_examples/launch/rgbdslam_datasets.launch.py @@ -1,47 +1,72 @@ # Example to run rgbd datasets: +# # [ROS1] Prepare ROS1 rosbag for conversion to ROS2 # $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag # $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag +# $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/45e6ac01541973a17505000fc8c7ca399ae275eb/tum_rename_world_kinect_frame.py +# $ python3 tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag # $ wget https://raw.githubusercontent.com/srv/srv_tools/kinetic/bag_tools/scripts/change_frame_id.py +# # Edit change_frame_id.py, remove/comment lines beginning with "PKG" and "import roslib", change line "Exception, e" to "Exception" # $ roscore # $ python3 change_frame_id.py -o rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag -i rgbd_dataset_freiburg3_long_office_household.bag -f openni_rgb_optical_frame -t /camera/rgb/image_color +# # [ROS2] # $ sudo pip install rosbags # See https://docs.openvins.com/dev-ros1-to-ros2.html -# $ rosbags-convert rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag - +# $ rosbags-convert --src rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag --dst rgbd_dataset_freiburg3_long_office_household_frameid_fixed +# # $ ros2 launch rtabmap_examples rgbdslam_datasets.launch.py # $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed # $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock - +# +# To get RMSE after the run: +# $ rtabmap-report ~/.ros/rtabmap.db from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.actions import SetParameter def generate_launch_description(): - parameters=[{ + odom_parameters=[{ + 'frame_id':'kinect', + # ground truth here is just used to align odometry with ground truth's first pose + 'ground_truth_frame_id':'world', + 'ground_truth_base_frame_id':'kinect_gt', + 'keep_color': True, + 'wait_for_transform': 0.5, + # RTAB-Map's parameters should all be string type: + 'Odom/Strategy':'0', + 'Odom/ResetCountdown':'15', + 'Odom/GuessSmoothingDelay':'0', + }] + slam_parameters=[{ 'frame_id':'kinect', - 'subscribe_depth':True, + # Record ground truth to compute RMSE + 'ground_truth_frame_id':'world', + 'ground_truth_base_frame_id':'kinect_gt', + 'subscribe_rgb':False, + 'subscribe_depth':False, + 'subscribe_rgbd':True, 'subscribe_odom_info':True, # RTAB-Map's parameters should all be string type: - 'Odom/Strategy':'0', - 'Odom/ResetCountdown':'15', - 'Odom/GuessSmoothingDelay':'0', + 'Mem/UseOdomFeatures': 'true', 'Rtabmap/StartNewMapOnLoopClosure':'true', 'RGBD/CreateOccupancyGrid':'false', 'Rtabmap/CreateIntermediateNodes':'true', 'RGBD/LinearUpdate':'0', 'RGBD/AngularUpdate':'0'}] - remappings=[ + odom_remappings=[ ('rgb/image', '/camera/rgb/image_color'), ('rgb/camera_info', '/camera/rgb/camera_info'), ('depth/image', '/camera/depth/image')] + + # We will use the output of odometry to avoid re-extracting + # the same features on slam side. + slam_remappings=[ + ("rgbd_image", "odom_rgbd_image")] return LaunchDescription([ @@ -52,19 +77,19 @@ def generate_launch_description(): # Nodes to launch Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', - parameters=parameters, - remappings=remappings), + parameters=odom_parameters, + remappings=odom_remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=parameters, - remappings=remappings, + parameters=slam_parameters, + remappings=slam_remappings, arguments=['-d']), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=parameters, - remappings=remappings), + parameters=slam_parameters, + remappings=slam_remappings), # /tf topic is missing in the converted ROS2 bag, create a fake tf Node( diff --git a/rtabmap_examples/launch/rtabmap_D405x2.launch.py b/rtabmap_examples/launch/rtabmap_D405x2.launch.py index 9d1de478..7a8fda54 100644 --- a/rtabmap_examples/launch/rtabmap_D405x2.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x2.launch.py @@ -29,7 +29,7 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): config_rviz = os.path.join( - get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x2_config.rviz') + get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x2_config.rviz') rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', output='screen', diff --git a/rtabmap_examples/launch/rtabmap_D405x3.launch.py b/rtabmap_examples/launch/rtabmap_D405x3.launch.py index 168bc40d..f2d6f61f 100644 --- a/rtabmap_examples/launch/rtabmap_D405x3.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x3.launch.py @@ -31,7 +31,7 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): config_rviz = os.path.join( - get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x3_config.rviz') + get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x3_config.rviz') rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', output='screen', diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py deleted file mode 100644 index dfc6927d..00000000 --- a/rtabmap_examples/launch/vlp16.launch.py +++ /dev/null @@ -1,126 +0,0 @@ -# Example: -# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py -# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py -# -# SLAM: -# $ ros2 launch rtabmap_examples vlp16.launch.py - - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - - use_sim_time = LaunchConfiguration('use_sim_time') - deskewing = LaunchConfiguration('deskewing') - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'deskewing', default_value='true', - description='Enable lidar deskewing'), - - # Nodes to launch - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'deskewing':deskewing, - 'use_sim_time':use_sim_time, - # RTAB-Map's internal parameters are strings: - 'Icp/PointToPlane': 'true', - 'Icp/Iterations': '10', - 'Icp/VoxelSize': '0.1', - 'Icp/Epsilon': '0.001', - 'Icp/PointToPlaneK': '20', - 'Icp/PointToPlaneRadius': '0', - 'Icp/MaxTranslation': '2', - 'Icp/MaxCorrespondenceDistance': '1', - 'Icp/Strategy': '1', - 'Icp/OutlierRatio': '0.7', - 'Icp/CorrespondenceRatio': '0.01', - 'Odom/ScanKeyFrameThr': '0.4', - 'OdomF2M/ScanSubtractRadius': '0.1', - 'OdomF2M/ScanMaxSize': '15000', - 'OdomF2M/BundleAdjustment': 'false' - }], - remappings=[ - ('scan_cloud', '/velodyne_points') - ]), - - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{ - 'max_clouds':10, - 'fixed_frame_id':'', - 'use_sim_time':use_sim_time, - }], - remappings=[ - ('cloud', 'odom_filtered_input_scan') - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'wait_for_transform':0.2, - 'use_sim_time':use_sim_time, - # RTAB-Map's internal parameters are strings: - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/VoxelSize': '0.1', - 'Icp/PointToPlaneK': '20', - 'Icp/PointToPlaneRadius': '0', - 'Icp/PointToPlane': 'true', - 'Icp/Iterations': '10', - 'Icp/Epsilon': '0.001', - 'Icp/MaxTranslation': '3', - 'Icp/MaxCorrespondenceDistance': '1', - 'Icp/Strategy': '1', - 'Icp/OutlierRatio': '0.7', - 'Icp/CorrespondenceRatio': '0.2' - }], - remappings=[ - ('scan_cloud', 'assembled_cloud') - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'use_sim_time':use_sim_time, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), - ]) - - diff --git a/rtabmap_examples/launch/vlp16_zed.launch.py b/rtabmap_examples/launch/vlp16_zed.launch.py new file mode 100644 index 00000000..67267e1f --- /dev/null +++ b/rtabmap_examples/launch/vlp16_zed.launch.py @@ -0,0 +1,121 @@ +# Example using zed odometry for lidar deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i +# +# To use only zed's imu for deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false +# + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def launch_setup(context: LaunchContext, *args, **kwargs): + + assemble = LaunchConfiguration('assemble').perform(context) + assemble = assemble == 'true' or assemble == 'True' + + lidar3d_launch_file = 'lidar3d.launch.py' + if assemble: + lidar3d_launch_file = 'lidar3d_assemble.launch.py' + + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) + use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True' + + fixed_frame_id = '' + if use_zed_odometry: + fixed_frame_id = 'odom' + + # Hack to override grab_resolution parameter without changing any files + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'") + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_driver'), 'launch'), + '/velodyne_driver_node-VLP16-launch.py']), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_pointcloud'), 'launch'), + '/velodyne_transform_node-VLP16-launch.py']), + ), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame + 'publish_map_tf': 'false'}.items(), + ), + + # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it) + Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]), + + # Sync rgb/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync': False}], + remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered')]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('rtabmap_examples'), 'launch'), + '/', lidar3d_launch_file]), + launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'), + 'localization': LaunchConfiguration('localization'), + 'frame_id': 'zed_camera_link', + 'lidar_topic': 'velodyne_points', + 'imu_topic': '/zed/zed_node/imu/data', + 'rgbd_image_topic': 'rgbd_image', + 'fixed_frame_id': fixed_frame_id}.items()), + ] + +def generate_launch_description(): + return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + + DeclareLaunchArgument( + 'use_zed_odometry', default_value='true', + description='Use ZED\'s odometry for deskewing.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'assemble', default_value='false', + description='Assemble ALL lidar scans.'), + + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py new file mode 100644 index 00000000..80ce57a5 --- /dev/null +++ b/rtabmap_examples/launch/zed.launch.py @@ -0,0 +1,101 @@ +# Requirements: +# A ZED camera +# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper) +# Example: +# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import UnlessCondition + +import tempfile + +parameters = [] +remappings = [] + +def launch_setup(context: LaunchContext, *args, **kwargs): + + # Hack to override grab_resolution parameter without changing any files + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'") + + parameters=[{'frame_id':'zed_camera_link', + 'subscribe_rgbd':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', '/zed/zed_node/imu/data')] + + if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]: + remappings.append(('odom', '/zed/zed_node/odom')) + else: + parameters.append({'subscribe_odom_info': True}) + + return [ + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': LaunchConfiguration('use_zed_odometry'), + 'publish_map_tf': 'false'}.items(), + ), + + # Sync rgb/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=parameters, + remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')), + parameters=parameters, + remappings=remappings,), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ] + + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_zed_odometry', default_value='false', + description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 0dba0037..bb80df0f 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -2,7 +2,7 @@ rtabmap_examples - 0.21.5 + 0.22.0 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/README.md b/rtabmap_launch/README.md new file mode 100644 index 00000000..449543b6 --- /dev/null +++ b/rtabmap_launch/README.md @@ -0,0 +1,40 @@ + + +# Usage + +`rtabmap.launch` from ros1 has been ported to ROS2 as `rtabmap.launch.py` with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this: + +```bash +roslaunch zed_wrapper zed_no_tf.launch + +roslaunch rtabmap_ros rtabmap.launch \ + rtabmap_args:="--delete_db_on_start" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/rgb/camera_info \ + frame_id:=base_link \ + approx_sync:=false \ + wait_imu_to_init:=true \ + imu_topic:=/zed_node/imu/data + +``` + +The ROS2 equivalent is (using latest zed_wrapper launch file): + +```bash +ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i \ + publish_tf:=false \ + publish_map_tf:=false + +ros2 launch rtabmap_launch rtabmap.launch.py \ + rtabmap_args:="--delete_db_on_start" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/rgb/camera_info \ + frame_id:=zed_camera_link \ + approx_sync:=false \ + wait_imu_to_init:=true \ + imu_topic:=/zed/zed_node/imu/data \ + rviz:=true +``` + diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 50db0297..26e4d549 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -45,6 +45,7 @@ def launch_setup(context, *args, **kwargs): DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''), DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''), DeclareLaunchArgument('args', default_value=LaunchConfiguration('rtabmap_args'), description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'), + DeclareLaunchArgument('sync_queue_size', default_value=LaunchConfiguration('queue_size'), description='Queue size of topic synchronizers.'), DeclareLaunchArgument('qos_image', default_value=LaunchConfiguration('qos'), description='Specific QoS used for image input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_camera_info', default_value=LaunchConfiguration('qos'), description='Specific QoS used for camera info input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_scan', default_value=LaunchConfiguration('qos'), description='Specific QoS used for scan input data: 0=system default, 1=Reliable, 2=Best Effort.'), @@ -53,6 +54,8 @@ def launch_setup(context, *args, **kwargs): 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!'), DeclareLaunchArgument('depth_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('depth_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('depth_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'), @@ -82,11 +85,13 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_sync', executable='rgbd_sync', name="rgbd_sync", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])), parameters=[{ "approx_sync": LaunchConfiguration('approx_rgbd_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos": LaunchConfiguration('qos_image'), "qos_camera_info": LaunchConfiguration('qos_camera_info'), "depth_scale": LaunchConfiguration('depth_scale')}], @@ -116,11 +121,13 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])), parameters=[{ "approx_sync": LaunchConfiguration('approx_rgbd_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos": LaunchConfiguration('qos_image'), "qos_camera_info": LaunchConfiguration('qos_camera_info')}], remappings=[ @@ -134,6 +141,7 @@ def launch_setup(context, *args, **kwargs): # Relay rgbd_image Node( package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])), parameters=[{ "qos": LaunchConfiguration('qos_image')}], @@ -143,6 +151,7 @@ def launch_setup(context, *args, **kwargs): namespace=LaunchConfiguration('namespace')), Node( package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay_uncompress", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])), parameters=[{ "uncompress": True, @@ -155,6 +164,7 @@ def launch_setup(context, *args, **kwargs): # RGB-D odometry Node( package='rtabmap_odom', executable='rgbd_odometry', name="rgbd_odometry", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -164,10 +174,12 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), "config_path": LaunchConfiguration('cfg').perform(context), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos": LaunchConfiguration('qos_image'), "qos_camera_info": LaunchConfiguration('qos_camera_info'), "qos_imu": LaunchConfiguration('qos_imu'), @@ -182,13 +194,14 @@ 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')), # Stereo odometry Node( package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen", + emulate_tty=True, condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -198,10 +211,12 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), "config_path": LaunchConfiguration('cfg').perform(context), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos": LaunchConfiguration('qos_image'), "qos_camera_info": LaunchConfiguration('qos_camera_info'), "qos_imu": LaunchConfiguration('qos_imu'), @@ -217,13 +232,14 @@ 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')), # ICP odometry Node( package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen", + emulate_tty=True, condition=IfCondition(LaunchConfiguration('icp_odometry')), parameters=[{ "frame_id": LaunchConfiguration('frame_id'), @@ -233,9 +249,11 @@ def launch_setup(context, *args, **kwargs): "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'), + "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'), "approx_sync": LaunchConfiguration('approx_sync'), "config_path": LaunchConfiguration('cfg').perform(context), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos": LaunchConfiguration('qos_image'), "qos_imu": LaunchConfiguration('qos_imu'), "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), @@ -246,12 +264,13 @@ 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')), Node( package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen", + emulate_tty=True, parameters=[{ "subscribe_depth": LaunchConfiguration('depth'), "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), @@ -266,6 +285,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'), @@ -275,7 +295,8 @@ def launch_setup(context, *args, **kwargs): "database_path": LaunchConfiguration('database_path'), "approx_sync": LaunchConfiguration('approx_sync'), "config_path": LaunchConfiguration('cfg').perform(context), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos_image": LaunchConfiguration('qos_image'), "qos_scan": LaunchConfiguration('qos_scan'), "qos_odom": LaunchConfiguration('qos_odom'), @@ -290,6 +311,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')), @@ -306,13 +328,15 @@ def launch_setup(context, *args, **kwargs): ("tag_detections", LaunchConfiguration('tag_topic')), ("fiducial_transforms", LaunchConfiguration('fiducial_topic')), ("odom", LaunchConfiguration('odom_topic')), - ("imu", LaunchConfiguration('imu_topic'))], - arguments=[LaunchConfiguration("args")], + ("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')), Node( package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output='screen', + emulate_tty=True, parameters=[{ "subscribe_depth": LaunchConfiguration('depth'), "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), @@ -326,7 +350,8 @@ def launch_setup(context, *args, **kwargs): "odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context), "wait_for_transform": LaunchConfiguration('wait_for_transform'), "approx_sync": LaunchConfiguration('approx_sync'), - "queue_size": LaunchConfiguration('queue_size'), + "topic_queue_size": LaunchConfiguration('topic_queue_size'), + "sync_queue_size": LaunchConfiguration('sync_queue_size'), "qos_image": LaunchConfiguration('qos_image'), "qos_scan": LaunchConfiguration('qos_scan'), "qos_odom": LaunchConfiguration('qos_odom'), @@ -346,7 +371,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( @@ -355,12 +380,15 @@ def launch_setup(context, *args, **kwargs): arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), Node( package='rtabmap_util', executable='point_cloud_xyzrgb', name="point_cloud_xyzrgb", output='screen', + emulate_tty=True, condition=IfCondition(LaunchConfiguration("rviz")), parameters=[{ "decimation": 4, "voxel_size": 0.0, "approx_sync": LaunchConfiguration('approx_sync'), - "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval') + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info') }], remappings=[ ('left/image', LaunchConfiguration('left_image_topic_relay')), @@ -391,6 +419,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,17 +429,22 @@ 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.'), - DeclareLaunchArgument('queue_size', default_value='10', description=''), - DeclareLaunchArgument('qos', default_value='1', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), + DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'), + DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'), + DeclareLaunchArgument('qos', default_value='0', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''), DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'), DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'), 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)'), @@ -460,11 +495,12 @@ def generate_launch_description(): DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''), DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''), DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''), - + # imu DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'), DeclareLaunchArgument('wait_imu_to_init', default_value='false', description=''), - + DeclareLaunchArgument('always_check_imu_tf', default_value='true', description='The odometry node will always check if TF between IMU frame and base frame has changed. If false, it is checked till a valid transform is initialized.'), + # User Data DeclareLaunchArgument('subscribe_user_data', default_value='false', description='User data synchronized subscription.'), DeclareLaunchArgument('user_data_topic', default_value='/user_data', description=''), diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index fb19c7ec..f674e8f9 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -2,7 +2,7 @@ rtabmap_launch - 0.21.5 + 0.22.0 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt index 7ae7eead..01e7cebb 100644 --- a/rtabmap_msgs/CMakeLists.txt +++ b/rtabmap_msgs/CMakeLists.txt @@ -10,6 +10,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + rcutils_LIB NAMES rcutils + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + ################## ## Dependencies ## ################## diff --git a/rtabmap_msgs/msg/OdomInfo.msg b/rtabmap_msgs/msg/OdomInfo.msg index 369334f3..870963c8 100644 --- a/rtabmap_msgs/msg/OdomInfo.msg +++ b/rtabmap_msgs/msg/OdomInfo.msg @@ -18,6 +18,8 @@ int32 local_key_frames int32 local_bundle_outliers int32 local_bundle_constraints float32 local_bundle_time +float32 local_bundle_avg_inlier_distance +int32 local_bundle_max_key_frames_for_inlier bool key_frame_added float32 time_estimation float32 time_particle_filtering diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index f82a254f..136fb15a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -2,7 +2,7 @@ rtabmap_msgs - 0.21.5 + 0.22.0 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe @@ -14,6 +14,8 @@ rosidl_default_generators + ros_environment + builtin_interfaces std_msgs std_srvs diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index f9859b44..96130502 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/rtabmap_odom/CMakeLists.txt @@ -10,6 +10,15 @@ if(POLICY CMP0074) cmake_policy(SET CMP0074 NEW) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake_ros REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_geometry REQUIRED) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index ae2f7d5f..4a1b7a81 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -59,7 +60,7 @@ class Odometry; namespace rtabmap_odom { -class OdometryROS : public rclcpp::Node +class OdometryROS : public rclcpp::Node, public UThread { public: @@ -98,12 +99,18 @@ protected: private: + virtual void mainLoop(); + virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} virtual void onOdomInit() {} void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); +protected: + rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_; + void tick(const rclcpp::Time & stamp); + private: rtabmap::Odometry * odometry_; @@ -147,6 +154,15 @@ private: std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; rclcpp::Subscription::SharedPtr imuSub_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + + // Safe-threading + UMutex imuMutex_; + UMutex dataMutex_; + USemaphore dataReady_; + rtabmap::SensorData dataToProcess_; + std_msgs::msg::Header dataHeaderToProcess_; + bool bufferedDataToProcess_; bool paused_; int resetCountdown_; @@ -157,6 +173,7 @@ private: rtabmap::Transform guess_; rtabmap::Transform guessPreviousPose_; double previousStamp_; + double previousClockTime_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; @@ -164,11 +181,14 @@ private: bool compressionParallelized_; int odomStrategy_; bool waitIMUToinit_; + bool alwaysCheckImuTf_; bool imuProcessed_; - std::map imus_; - std::pair bufferedData_; + int processedMsgs_; + int droppedMsgs_; + std::map imus_; std::string configPath_; rtabmap::Transform initialPose_; + rtabmap::Transform imuLocalTransform_; rtabmap_util::ULogToRosout ulogToRosout_; @@ -176,11 +196,13 @@ private: { public: OdomStatusTask(); - void setStatus(bool isLost); + void setStatus(bool isLost, int processedMsgs, int droppedMsgs); void run(diagnostic_updater::DiagnosticStatusWrapper &stat); private: bool lost_; bool dataReceived_; + int processedMsgs_; + int droppedMsgs_; }; OdomStatusTask statusDiagnostic_; std::unique_ptr syncDiagnostic_; diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 090c3beb..b8aee9ee 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 { @@ -144,7 +148,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/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index 3f5dddf4..4167e738 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 @@ -142,7 +146,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_odom/package.xml b/rtabmap_odom/package.xml index f9d7f40b..c51f592f 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -2,7 +2,7 @@ rtabmap_odom - 0.21.5 + 0.22.0 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_geometry laser_geometry diff --git a/rtabmap_odom/src/ICPOdometryNode.cpp b/rtabmap_odom/src/ICPOdometryNode.cpp index da85a658..f83264d0 100644 --- a/rtabmap_odom/src/ICPOdometryNode.cpp +++ b/rtabmap_odom/src/ICPOdometryNode.cpp @@ -70,7 +70,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 0b8235e5..c9955d99 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 @@ -80,7 +84,11 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o paused_(false), resetCountdown_(0), resetCurrentCount_(0), + stereoParams_(false), + visParams_(false), + icpParams_(false), previousStamp_(0.0), + previousClockTime_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), @@ -88,11 +96,16 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), waitIMUToinit_(false), + alwaysCheckImuTf_(true), imuProcessed_(false), + processedMsgs_(0), + droppedMsgs_(0), configPath_(), initialPose_(Transform::getIdentity()), ulogToRosout_(this) { + dataCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + int qos = this->declare_parameter("qos", (int)qos_); qos_ = (rmw_qos_reliability_policy_t)qos; @@ -107,11 +120,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o odomSensorDataFeaturesPub_ = create_publisher("odom_sensor_data/features", rclcpp::QoS(1).reliability(qos_)); odomSensorDataCompressedPub_ = create_publisher("odom_sensor_data/compressed", rclcpp::QoS(1).reliability(qos_)); - tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); + tfBuffer_ = std::make_shared(get_clock()); tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); @@ -140,6 +149,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_); + alwaysCheckImuTf_ = this->declare_parameter("always_check_imu_tf", alwaysCheckImuTf_); + configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir()); if(configPath_.size() && configPath_.at(0) != '/') @@ -194,12 +205,14 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf = %s", alwaysCheckImuTf_?"true":"false"); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str()); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false"); } OdometryROS::~OdometryROS() { + this->join(true); delete odometry_; } @@ -361,14 +374,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams) Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); if(waitIMUToinit_) { - int queueSize = 10; - this->get_parameter_or("queue_size", queueSize, queueSize); + imuCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = imuCallbackGroup_; + int queueSize = this->declare_parameter("imu_queue_size", 200); int qosImu = this->declare_parameter("qos_imu", (int)qos_); - imuSub_ = create_subscription("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1)); + imuSub_ = create_subscription("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1), options); RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name()); RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu); + RCLCPP_INFO(this->get_logger(), "odometry: imu_queue_size = %d", queueSize); } + this->start(); + onOdomInit(); } @@ -404,88 +422,195 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) if(!this->isPaused()) { double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp); - rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); - if(this->frameId().compare(msg->header.frame_id) != 0) + //RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds()); + { - localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_); + UScopeMutex m(imuMutex_); + + if(!imuProcessed_ && imus_.empty()) + { + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_); + if(localTransform.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.", + this->frameId().c_str(), msg->header.frame_id.c_str()); + return; + } + } + + imus_.insert(std::make_pair(stamp, msg)); + + if(imus_.size() > 1000) + { + RCLCPP_WARN(this->get_logger(), "Dropping imu data!"); + imus_.erase(imus_.begin()); + } } - if(localTransform.isNull()) + if(dataMutex_.lockTry() == 0) { - RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f", - msg->header.frame_id.c_str(), this->frameId().c_str(), stamp); - return; - } - - IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w), - cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(), - cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z), - cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(), - cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z), - cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), - localTransform); - - imus_.insert(std::make_pair(stamp, imu)); - //RCLCPP_WARN(get_logger(), "Received imu: %f", stamp); - - if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp()) - { - SensorData data = bufferedData_.first; - bufferedData_.first = SensorData(); - processData(data, bufferedData_.second); - } - - if(imus_.size() > 1000) - { - imus_.erase(imus_.begin()); + if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp) + { + bufferedDataToProcess_ = false; + dataReady_.release(); + } + dataMutex_.unlock(); } } } void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { - if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + if(dataMutex_.lockTry() == 0) { - RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); - return; - } - - if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) - { - //RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp)); - - // keep in cache to process later when we will receive imu msgs - if(bufferedData_.first.isValid()) - { - RCLCPP_ERROR(this->get_logger(), "Overwriting previous data! Make sure IMU is " - "published faster than data rate. (last image stamp " - "buffered=%f and new one is %f, last imu stamp received=%f)", - bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(bufferedDataToProcess_) { + RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp)); + ++droppedMsgs_; } - bufferedData_.first = data; - bufferedData_.second = header; + dataToProcess_ = data; + dataHeaderToProcess_ = header; + bufferedDataToProcess_ = false; + dataReady_.release(); + dataMutex_.unlock(); + ++processedMsgs_; + } + else + { + //RCLCPP_WARN(get_logger(), "Dropping image/scan data"); + ++droppedMsgs_; + } +} + +void OdometryROS::mainLoopKill() +{ + // in case we were waiting, unblock thread + dataReady_.release(); +} + +void OdometryROS::mainLoop() +{ + dataReady_.acquire(); + + if(!this->isRunning()) + { + // thread killed return; } - // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); - if(iterEnd!= imus_.end()) + + UScopeMutex lock(dataMutex_); + + // aliases + SensorData & data = dataToProcess_; + std_msgs::msg::Header & header = dataHeaderToProcess_; + + std::vector > imus; { - ++iterEnd; - } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + UScopeMutex m(imuMutex_); + + if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + { + RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); + return; + } + + if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) + { + RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; + return; + } + // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) + std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + if(iterEnd!= imus_.end()) + { + ++iterEnd; + } + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + { + imus.push_back(*iter); + imus_.erase(iter++); + } + } // end imu lock + + bool imuWarnShown = false; + for(size_t i=0; ifirst); - SensorData dataIMU(iter->second, 0, iter->first); + if((alwaysCheckImuTf_ && !imuWarnShown) || imuLocalTransform_.isNull()) + { + if(this->frameId().compare(imus[i].second->header.frame_id) != 0) + { + // We should not have to wait for IMU TF (imu delay <<< sensor data delay), so don't + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), imus[i].second->header.frame_id, imus[i].second->header.stamp, *tfBuffer_, 0); + if(localTransform.isNull()) + { + if(imuLocalTransform_.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. All IMU msgs up to sensor data time %f are skipped! If IMU TF is not static, make sure to publish it before the the imu topic is published.", + imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp), data.stamp()); + break; + } else if(!imuWarnShown) { + imuWarnShown = true; // show only one time + RCLCPP_WARN(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. We will use latest known IMU local transform (if TF between camera/lidar and the IMU is static, you can safely ignore this warning and set always_check_imu_tf to false).", + imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp)); + } + } + else { + imuLocalTransform_ = localTransform; + } + } + else if(imuLocalTransform_.isNull()) + { + imuLocalTransform_.setIdentity(); + } + } + + IMU imu(cv::Vec4d(imus[i].second->orientation.x, imus[i].second->orientation.y, imus[i].second->orientation.z, imus[i].second->orientation.w), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->orientation_covariance.data()).clone(), + cv::Vec3d(imus[i].second->angular_velocity.x, imus[i].second->angular_velocity.y, imus[i].second->angular_velocity.z), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->angular_velocity_covariance.data()).clone(), + cv::Vec3d(imus[i].second->linear_acceleration.x, imus[i].second->linear_acceleration.y, imus[i].second->linear_acceleration.z), + cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->linear_acceleration_covariance.data()).clone(), + imuLocalTransform_); + + SensorData dataIMU(imu, 0, imus[i].first); odometry_->process(dataIMU); - imus_.erase(iter++); imuProcessed_ = true; } - //RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp)); - Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { - if(previousStamp_>0.0 && previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp)) + // Detect time jump in the past + double clockNow = now().seconds(); + if(previousClockTime_ > clockNow) + { + RCLCPP_WARN(this->get_logger(), "Odometry: Detected jump back in time of %f sec. Odometry is " + "automatically reset to latest computed pose!", + previousClockTime_ - clockNow); + SensorData dataCpy = dataToProcess_; + std_msgs::msg::Header headerCpy = dataHeaderToProcess_; + double previousCpy = previousClockTime_; + this->reset(odometry_->getPose()); + if(clockNow > rtabmap_conversions::timestampFromROS(headerCpy.stamp)) { + // new frame is using new clock, process it now + dataToProcess_ = dataCpy; + dataHeaderToProcess_ = headerCpy; + dataReady_.release(); + RCLCPP_WARN(this->get_logger(), "Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", + rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow); + } + else { + // skip that old frame + RCLCPP_WARN(this->get_logger(), "Odometry: skipping frame: %f (clock previous=%f, new=%f)", + rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow); + } + previousClockTime_ = clockNow; + return; + } + previousClockTime_ = clockNow; + + if(previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp)) { RCLCPP_WARN(this->get_logger(), "Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). " "New stamp should be always greater than previous stamp. This new data is ignored.", @@ -585,7 +710,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + previousClockTime_ - time_now); + } } guessPreviousPose_ = guessCurrentPose; return; @@ -603,7 +738,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_; // process data - rclcpp::Time timeStart = now(); + rclcpp::Time timeStart = rclcpp::Clock().now(); rtabmap::OdometryInfo info; if(!groundTruth.isNull()) { @@ -639,11 +774,30 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + previousClockTime_ - time_now); + } } else { - tfBroadcaster_->sendTransform(poseMsg); + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(poseMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.", + poseMsg.header.frame_id.c_str(), + poseMsg.child_frame_id.c_str(), + previousClockTime_ - time_now); + } } } @@ -835,7 +989,18 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because its stamp (%f) is greater " + "than current time (%f), possible time jump happened!", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + rtabmap_conversions::timestampFromROS(correctionMsg.header.stamp), + time_now); + } } } @@ -848,10 +1013,9 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); } - else + else if(--resetCurrentCount_>0) { RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); - --resetCurrentCount_; } if(resetCurrentCount_ == 0 || tooOldPreviousData) @@ -879,6 +1043,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h odometry_->reset(tfPose); } } + // Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features). + // This will make sure we keep updating to latest guess pose. + if(resetCurrentCount_ == 0) { + ++resetCurrentCount_; + } } } @@ -925,124 +1094,124 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h } } - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) + if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0) { - if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0) + rtabmap_msgs::msg::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + if(odomSensorDataPub_->get_subscription_count()>0) { - rtabmap_msgs::msg::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - if(odomSensorDataPub_->get_subscription_count()>0) - { - odomSensorDataPub_->publish(msg); - } - if(odomSensorDataFeaturesPub_->get_subscription_count()>0) - { - // remove data - msg.left = sensor_msgs::msg::Image(); - msg.right = sensor_msgs::msg::Image(); - msg.laser_scan = sensor_msgs::msg::PointCloud2(); - msg.grid_ground.clear(); - msg.grid_obstacles.clear(); - msg.grid_empty_cells.clear(); - odomSensorDataFeaturesPub_->publish(msg); - } + odomSensorDataPub_->publish(msg); } - if(odomSensorDataCompressedPub_->get_subscription_count()>0) + if(odomSensorDataFeaturesPub_->get_subscription_count()>0) { - cv::Mat compressedImage; - cv::Mat compressedDepth; - cv::Mat compressedScan; - if(compressionParallelized_) - { - rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); - rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); - if(!data.imageRaw().empty()) - { - ctImage.start(); - } - if(!data.depthOrRightRaw().empty()) - { - ctDepth.start(); - } - if(!data.laserScanRaw().isEmpty()) - { - ctLaserScan.start(); - } - ctImage.join(); - ctDepth.join(); - ctLaserScan.join(); - - compressedImage = ctImage.getCompressedData(); - compressedDepth = ctDepth.getCompressedData(); - compressedScan = ctLaserScan.getCompressedData(); - } - else - { - compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); - compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - compressedScan = compressData2(data.laserScanRaw().data()); - } - if(!compressedImage.empty() && !data.stereoCameraModels().empty()) - { - data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); - } - else if(!compressedImage.empty() && !data.cameraModels().empty()) - { - data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); - } - if(!compressedScan.empty()) - { - data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? - LaserScan(compressedScan, - data.laserScanRaw().maxPoints(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().format(), - data.laserScanRaw().localTransform()): - LaserScan(compressedScan, - data.laserScanRaw().format(), - data.laserScanRaw().rangeMin(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().angleMin(), - data.laserScanRaw().angleMax(), - data.laserScanRaw().angleIncrement(), - data.laserScanRaw().localTransform()), false); - } - rtabmap_msgs::msg::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - odomSensorDataCompressedPub_->publish(msg); + // remove data + msg.left = sensor_msgs::msg::Image(); + msg.right = sensor_msgs::msg::Image(); + msg.laser_scan = sensor_msgs::msg::PointCloud2(); + msg.grid_ground.clear(); + msg.grid_obstacles.clear(); + msg.grid_empty_cells.clear(); + odomSensorDataFeaturesPub_->publish(msg); } - - if(visParams_) - { - if(icpParams_) - { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); - } - else - { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); - } - } - else // if(icpParams_) - { - RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); - } - - statusDiagnostic_.setStatus(pose.isNull()); - if(syncDiagnostic_.get() && !pose.isNull()) - { - double curentRate = 1.0/(this->now()-timeStart).seconds(); - syncDiagnostic_->tick(header.stamp, - maxUpdateRate_>0 ? maxUpdateRate_: - expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate); - } - - previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp); } + if(odomSensorDataCompressedPub_->get_subscription_count()>0) + { + cv::Mat compressedImage; + cv::Mat compressedDepth; + cv::Mat compressedScan; + if(compressionParallelized_) + { + rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); + rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); + if(!data.imageRaw().empty()) + { + ctImage.start(); + } + if(!data.depthOrRightRaw().empty()) + { + ctDepth.start(); + } + if(!data.laserScanRaw().isEmpty()) + { + ctLaserScan.start(); + } + ctImage.join(); + ctDepth.join(); + ctLaserScan.join(); + + compressedImage = ctImage.getCompressedData(); + compressedDepth = ctDepth.getCompressedData(); + compressedScan = ctLaserScan.getCompressedData(); + } + else + { + compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); + compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + compressedScan = compressData2(data.laserScanRaw().data()); + } + if(!compressedImage.empty() && !data.stereoCameraModels().empty()) + { + data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); + } + else if(!compressedImage.empty() && !data.cameraModels().empty()) + { + data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); + } + if(!compressedScan.empty()) + { + data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? + LaserScan(compressedScan, + data.laserScanRaw().maxPoints(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().format(), + data.laserScanRaw().localTransform()): + LaserScan(compressedScan, + data.laserScanRaw().format(), + data.laserScanRaw().rangeMin(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().angleMin(), + data.laserScanRaw().angleMax(), + data.laserScanRaw().angleIncrement(), + data.laserScanRaw().localTransform()), false); + } + rtabmap_msgs::msg::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + odomSensorDataCompressedPub_->publish(msg); + } + + double delay = (now()-header.stamp).seconds(); + if(visParams_) + { + if(icpParams_) + { + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); + } + else + { + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); + } + } + else // if(icpParams_) + { + RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); + } + + statusDiagnostic_.setStatus(pose.isNull(), processedMsgs_, droppedMsgs_); + processedMsgs_ = 0; + droppedMsgs_ = 0; + if(syncDiagnostic_.get()) + { + double curentRate = 1.0/(rclcpp::Clock().now()-timeStart).seconds(); + syncDiagnostic_->tickOutput(header.stamp, + maxUpdateRate_>0 ? maxUpdateRate_: + expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: + previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate); + } + + previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp); } void OdometryROS::resetOdom( @@ -1066,14 +1235,21 @@ void OdometryROS::resetToPose( void OdometryROS::reset(const Transform & pose) { + UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; + previousClockTime_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; - bufferedData_.first= SensorData(); + dataToProcess_ = SensorData(); + dataHeaderToProcess_ = std_msgs::msg::Header(); + bufferedDataToProcess_ = false; + imuMutex_.lock(); imus_.clear(); + imuMutex_.unlock(); + imuLocalTransform_.setNull(); this->flushCallbacks(); } @@ -1145,13 +1321,17 @@ void OdometryROS::setLogError( OdometryROS::OdomStatusTask::OdomStatusTask() : diagnostic_updater::DiagnosticTask("Odom status"), lost_(false), - dataReceived_(false) + dataReceived_(false), + processedMsgs_(0), + droppedMsgs_(0) {} -void OdometryROS::OdomStatusTask::setStatus(bool isLost) +void OdometryROS::OdomStatusTask::setStatus(bool isLost, int processedMsgs, int droppedMsgs) { dataReceived_ = true; lost_ = isLost; + processedMsgs_ += processedMsgs; + droppedMsgs_ += droppedMsgs; } void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) @@ -1168,6 +1348,18 @@ void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrappe { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking."); } + stat.add("Topics Processed", processedMsgs_); + stat.add("Topics Dropped", droppedMsgs_); + processedMsgs_ = 0; + droppedMsgs_ = 0; +} + +void OdometryROS::tick(const rclcpp::Time & stamp) +{ + if(syncDiagnostic_.get()) + { + syncDiagnostic_->tickInput(stamp); + } } } diff --git a/rtabmap_odom/src/RGBDOdometryNode.cpp b/rtabmap_odom/src/RGBDOdometryNode.cpp index ad203b99..270d9ede 100644 --- a/rtabmap_odom/src/RGBDOdometryNode.cpp +++ b/rtabmap_odom/src/RGBDOdometryNode.cpp @@ -76,7 +76,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/StereoOdometryNode.cpp b/rtabmap_odom/src/StereoOdometryNode.cpp index d8976b7b..60a729ce 100644 --- a/rtabmap_odom/src/StereoOdometryNode.cpp +++ b/rtabmap_odom/src/StereoOdometryNode.cpp @@ -76,7 +76,10 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); - rclcpp::spin(std::make_shared(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 4babddd3..d30b68aa 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); - scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1)); - cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1)); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + + scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); @@ -277,6 +280,9 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan scan_sub_.reset(); return; } + + tick(scanMsg->header.stamp); + scanReceived_ = true; if(this->isPaused()) { @@ -342,6 +348,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan sensor_msgs::msg::PointCloud2 scanOutDeskewed; rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); + scanOutDeskewed.header.frame_id = scanMsg->header.frame_id; scanOut = scanOutDeskewed; } else @@ -520,6 +527,9 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p cloud_sub_.reset(); return; } + + tick(pointCloudMsg->header.stamp); + cloudReceived_ = true; if(this->isPaused()) { diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index b81e37a5..1da53fc4 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 @@ -59,7 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(10), + syncQueueSize_(5), keepColor_(false) { OdometryROS::init(false, true, false); @@ -89,25 +94,43 @@ void RGBDOdometry::onOdomInit() double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize_ = this->declare_parameter("queue_size", queueSize_); + topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize_ = queueSize; + RCLCPP_WARN(this->get_logger(), "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_); + } + syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_); int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos()); subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); - if(rgbdCameras <= 0) + if(rgbdCameras < 0) { - rgbdCameras = 1; + rgbdCameras = 0; } keepColor_ = this->declare_parameter("keep_color", keepColor_); + std::string rgbdTransport = this->declare_parameter("rgb_transport", std::string("raw")); + std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw")); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos()); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos()); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgb_transport = %s", rgbdTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str()); + + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; std::string subscribedTopic; std::string subscribedTopicsMsg; @@ -115,23 +138,23 @@ void RGBDOdometry::onOdomInit() { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras == 2) @@ -139,7 +162,7 @@ void RGBDOdometry::onOdomInit() if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -149,7 +172,7 @@ void RGBDOdometry::onOdomInit() else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -166,7 +189,7 @@ void RGBDOdometry::onOdomInit() if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -177,7 +200,7 @@ void RGBDOdometry::onOdomInit() else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -196,7 +219,7 @@ void RGBDOdometry::onOdomInit() if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -208,7 +231,7 @@ void RGBDOdometry::onOdomInit() else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -229,7 +252,7 @@ void RGBDOdometry::onOdomInit() if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -242,7 +265,7 @@ void RGBDOdometry::onOdomInit() else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -265,7 +288,7 @@ void RGBDOdometry::onOdomInit() if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -279,7 +302,7 @@ void RGBDOdometry::onOdomInit() else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -311,7 +334,7 @@ void RGBDOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -320,7 +343,7 @@ void RGBDOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -331,29 +354,40 @@ void RGBDOdometry::onOdomInit() } else { - image_transport::TransportHints hints(this); - image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport"); + image_transport::TransportHints depth_hints(this, "raw", "depth_transport"); + + std::string rgb_topic = get_node_base_interface()->resolve_topic_or_service_name( + "rgb/image", false, false + ); + std::string depth_topic = get_node_base_interface()->resolve_topic_or_service_name( + "depth/image", false, false + ); + + image_mono_sub_.subscribe(this, rgb_topic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_depth_sub_.subscribe(this, depth_topic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); if(approxSync) { - 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::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(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } subscribedTopic = image_mono_sub_.getSubscriber().getTopic(); - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + topicQueueSize_, + syncQueueSize_, image_mono_sub_.getSubscriber().getTopic().c_str(), image_depth_sub_.getSubscriber().getTopic().c_str(), info_sub_.getSubscriber()->get_topic_name()); @@ -548,6 +582,8 @@ void RGBDOdometry::callback( const sensor_msgs::msg::Image::ConstSharedPtr depth, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(1); @@ -576,6 +612,8 @@ void RGBDOdometry::callback( void RGBDOdometry::callbackRGBDX( const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images) { + tick(images->header.stamp); + if(!this->isPaused()) { if(images->rgbd_images.empty()) @@ -599,6 +637,8 @@ void RGBDOdometry::callbackRGBDX( void RGBDOdometry::callbackRGBD( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(1); @@ -615,6 +655,8 @@ void RGBDOdometry::callbackRGBD2( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(2); @@ -634,6 +676,8 @@ void RGBDOdometry::callbackRGBD3( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(3); @@ -656,6 +700,8 @@ void RGBDOdometry::callbackRGBD4( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(4); @@ -681,6 +727,8 @@ void RGBDOdometry::callbackRGBD5( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(5); @@ -709,6 +757,8 @@ void RGBDOdometry::callbackRGBD6( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector imageMsgs(6); @@ -737,20 +787,20 @@ void RGBDOdometry::flushCallbacks() 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(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::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(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -759,7 +809,7 @@ void RGBDOdometry::flushCallbacks() { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -768,7 +818,7 @@ void RGBDOdometry::flushCallbacks() { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -778,7 +828,7 @@ void RGBDOdometry::flushCallbacks() { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -788,7 +838,7 @@ void RGBDOdometry::flushCallbacks() { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -799,7 +849,7 @@ void RGBDOdometry::flushCallbacks() { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -810,7 +860,7 @@ void RGBDOdometry::flushCallbacks() { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -822,7 +872,7 @@ void RGBDOdometry::flushCallbacks() { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -834,7 +884,7 @@ void RGBDOdometry::flushCallbacks() { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -847,7 +897,7 @@ void RGBDOdometry::flushCallbacks() { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index c249ac40..c649f033 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 @@ -59,7 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(10), + syncQueueSize_(5), keepColor_(false) { OdometryROS::init(true, true, false); @@ -89,7 +94,17 @@ void StereoOdometry::onOdomInit() int rgbdCameras = 1; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize_ = this->declare_parameter("queue_size", queueSize_); + topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize_ = queueSize; + RCLCPP_WARN(this->get_logger(), "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_); + } + syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_); int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos()); subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); @@ -98,35 +113,39 @@ void StereoOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos()); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo); + RCLCPP_INFO(this->get_logger(), "StereoOdometry: topic_queue_size = %d", topicQueueSize_); + RCLCPP_INFO(this->get_logger(), "StereoOdometry: sync_queue_size = %d", syncQueueSize_); + RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos = %d", (int)qos()); + RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos_camera_info = %d", qosCamInfo); RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false"); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + std::string subscribedTopic; std::string subscribedTopicsMsg; if(subscribeRGBD) { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); + rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); } if(rgbdCameras == 2) @@ -134,7 +153,7 @@ void StereoOdometry::onOdomInit() if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -144,7 +163,7 @@ void StereoOdometry::onOdomInit() else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -161,7 +180,7 @@ void StereoOdometry::onOdomInit() if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -172,7 +191,7 @@ void StereoOdometry::onOdomInit() else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -191,7 +210,7 @@ void StereoOdometry::onOdomInit() if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -203,7 +222,7 @@ void StereoOdometry::onOdomInit() else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -224,7 +243,7 @@ void StereoOdometry::onOdomInit() if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -237,7 +256,7 @@ void StereoOdometry::onOdomInit() else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -260,7 +279,7 @@ void StereoOdometry::onOdomInit() if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -274,7 +293,7 @@ void StereoOdometry::onOdomInit() else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -307,7 +326,7 @@ void StereoOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = @@ -317,7 +336,7 @@ void StereoOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -329,29 +348,31 @@ void StereoOdometry::onOdomInit() else { image_transport::TransportHints hints(this); - imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); if(approxSync) { - 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::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(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } subscribedTopic = imageRectLeft_.getTopic(); - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + topicQueueSize_, + syncQueueSize_, imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getSubscriber()->get_topic_name(), @@ -693,6 +714,8 @@ void StereoOdometry::callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight) { + tick(imageRectLeft->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(1); @@ -723,6 +746,8 @@ void StereoOdometry::callback( void StereoOdometry::callbackRGBD( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(1); @@ -740,6 +765,8 @@ void StereoOdometry::callbackRGBD( void StereoOdometry::callbackRGBDX( const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images) { + tick(images->header.stamp); + if(!this->isPaused()) { if(images->rgbd_images.empty()) @@ -766,6 +793,8 @@ void StereoOdometry::callbackRGBD2( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(2); @@ -788,6 +817,8 @@ void StereoOdometry::callbackRGBD3( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(3); @@ -814,6 +845,8 @@ void StereoOdometry::callbackRGBD4( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(4); @@ -844,6 +877,8 @@ void StereoOdometry::callbackRGBD5( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(5); @@ -878,6 +913,8 @@ void StereoOdometry::callbackRGBD6( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { + tick(image->header.stamp); + if(!this->isPaused()) { std::vector leftMsgs(6); @@ -913,20 +950,20 @@ void StereoOdometry::flushCallbacks() 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(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::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(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -935,7 +972,7 @@ void StereoOdometry::flushCallbacks() { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); @@ -944,7 +981,7 @@ void StereoOdometry::flushCallbacks() { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -954,7 +991,7 @@ void StereoOdometry::flushCallbacks() { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -964,7 +1001,7 @@ void StereoOdometry::flushCallbacks() { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -975,7 +1012,7 @@ void StereoOdometry::flushCallbacks() { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -986,7 +1023,7 @@ void StereoOdometry::flushCallbacks() { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -998,7 +1035,7 @@ void StereoOdometry::flushCallbacks() { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1010,7 +1047,7 @@ void StereoOdometry::flushCallbacks() { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1023,7 +1060,7 @@ void StereoOdometry::flushCallbacks() { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index a4e6eb39..d56a425c 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -2,7 +2,7 @@ rtabmap_python - 0.21.5 + 0.22.0 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 316d805f..2da81cf3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -2,7 +2,7 @@ rtabmap_ros - 0.21.5 + 0.22.0 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index 95cf2f69..c1808ebe 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -5,6 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + message_filters_LIB NAMES message_filters + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) @@ -46,21 +55,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 +65,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 +78,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 +104,4 @@ install(TARGETS INCLUDES DESTINATION include ) -pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) - ament_package() 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/package.xml b/rtabmap_rviz_plugins/package.xml index 29f8660a..ada5c080 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -2,7 +2,7 @@ rtabmap_rviz_plugins - 0.21.5 + 0.22.0 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + pcl_conversions pluginlib rclcpp diff --git a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp index 87dc50fa..b66b6b31 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 ); @@ -316,7 +319,8 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map) cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f, cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); // convert back in /base_link frame - cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); + if(!cloud->empty()) + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); } if(!cloud->empty()) @@ -500,64 +504,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 +571,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/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index 946180d9..f53b7aaa 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -10,11 +10,19 @@ if(POLICY CMP0074) cmake_policy(SET CMP0074 NEW) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) 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 +36,13 @@ 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(aruco_msgs) +find_package(aruco_markers_msgs) +find_package(aruco_opencv_msgs) +find_package(ros2_aruco_interfaces) +find_package(nav2_msgs) IF(WIN32) add_compile_options(-bigobj) @@ -48,7 +57,6 @@ SET(Libraries cv_bridge geometry_msgs nav_msgs - nav2_msgs rclcpp rclcpp_components sensor_msgs @@ -62,6 +70,10 @@ SET(Libraries rtabmap_sync ) +if("$ENV{ROS_DISTRO}" STRLESS "jazzy") + add_definitions(-DPRE_ROS_JAZZY) +endif() + ########### ## Build ## ########### @@ -80,6 +92,59 @@ SET(Libraries ) ENDIF(apriltag_msgs_FOUND) +# If aruco_msgs is found, add definition +IF(aruco_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MSGS") +SET(Libraries + ${Libraries} + aruco_msgs +) +ENDIF(aruco_msgs_FOUND) + +# If aruco_opencv_msgs is found, add definition +IF(aruco_opencv_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_opencv_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS") +SET(Libraries + ${Libraries} + aruco_opencv_msgs +) +ENDIF(aruco_opencv_msgs_FOUND) + +# If aruco_markers_msgs is found, add definition +IF(aruco_markers_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_markers_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS") +SET(Libraries + ${Libraries} + aruco_markers_msgs +) +ENDIF(aruco_markers_msgs_FOUND) + +# If ros2_aruco_interfaces is found, add definition +IF(ros2_aruco_interfaces_FOUND) +MESSAGE(STATUS "WITH ros2_aruco_interfaces") +ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES") +SET(Libraries + ${Libraries} + ros2_aruco_interfaces +) +ENDIF(ros2_aruco_interfaces_FOUND) + +# If nav2_msgs is found, add definition +IF(nav2_msgs_FOUND) +MESSAGE(STATUS "WITH nav2_msgs") +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 +193,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..157a9b26 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -88,8 +88,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef WITH_ARUCO_MSGS +#include +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +#include +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +#include +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +#include +#endif + +#ifdef WITH_NAV2_MSGS #include #include +#endif //#define WITH_FIDUCIAL_MSGS #ifdef WITH_FIDUCIAL_MSGS @@ -109,13 +127,16 @@ 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); - bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom + bool odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp); // TF odom + // Callback called from sync thread virtual void commonMultiCameraCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -130,6 +151,7 @@ private: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); + // Callback called from sync thread void commonMultiCameraCallbackImpl( const std::string & odomFrameId, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -144,6 +166,7 @@ private: const std::vector > & localKeyPoints, const std::vector > & localPoints3d, const std::vector & localDescriptors); + // Callback called from sync thread virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -151,11 +174,13 @@ private: const sensor_msgs::msg::PointCloud2 & scan3dMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()); + // Callback called from sync thread virtual void commonOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg); + // Callback called from sync thread virtual void commonSensorDataCallback( const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg, const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, @@ -169,7 +194,20 @@ private: void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection); void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections); #ifdef WITH_APRILTAG_MSGS - void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections); + void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); + void apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MSGS + void arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + void arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + void arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + void arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg); #endif #ifdef WITH_FIDUCIAL_MSGS void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections); @@ -191,6 +229,8 @@ private: void goalNodeCallback(const rtabmap_msgs::msg::Goal::SharedPtr msg); void updateGoal(const rclcpp::Time & stamp); + void processAsync(); + void process( const rclcpp::Time & stamp, rtabmap::SensorData & data, @@ -246,12 +286,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); + 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); @@ -260,11 +302,14 @@ private: private: rtabmap::Rtabmap rtabmap_; bool paused_; + + UMutex lastPoseMutex_; rtabmap::Transform lastPose_; rclcpp::Time lastPoseStamp_; std::vector lastPoseVelocity_; + cv::Mat lastPoseCovariance_; bool lastPoseIntermediate_; - cv::Mat covariance_; + rtabmap::Transform currentMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_; bool latestNodeWasReached_; @@ -335,7 +380,7 @@ private: std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; - rclcpp::SyncParametersClient::SharedPtr parametersClient_; + rclcpp::AsyncParametersClient::SharedPtr parametersClient_; rclcpp::Subscription::SharedPtr parameterEventSub_; rclcpp::Service::SharedPtr updateSrv_; @@ -373,7 +418,10 @@ private: rclcpp::Service::SharedPtr octomapBinarySrv_; rclcpp::Service::SharedPtr octomapFullSrv_; #endif +#ifdef WITH_NAV2_MSGS rclcpp_action::Client::SharedPtr nav2Client_; + rclcpp_action::GoalUUID lastGoalSent_; +#endif std::thread* transformThread_; bool tfThreadRunning_; @@ -381,27 +429,52 @@ private: // for loop closure detection only image_transport::Subscriber defaultSub_; + rclcpp::CallbackGroup::SharedPtr userDataAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr userDataAsyncSub_; cv::Mat userData_; UMutex userDataMutex_; + rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr globalPoseAsyncSub_; - geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; + std::map globalPoses_; + UMutex globalPoseMutex_; + + rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; - rtabmap::GPS gps_; + std::map gps_; + UMutex gpsMutex_; + + rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; + rclcpp::Subscription::SharedPtr apriltagSub_; +#endif +#ifdef WITH_ARUCO_MSGS + rclcpp::Subscription::SharedPtr arucoSub_; +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + rclcpp::Subscription::SharedPtr arucoOpencvSub_; +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + rclcpp::Subscription::SharedPtr arucoMarkersSub_; +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + rclcpp::Subscription::SharedPtr arucoInterfacesSub_; #endif #ifdef WITH_FIDUCIAL_MSGS rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; #endif std::map > landmarks_; // id, - rclcpp::Subscription::SharedPtr imuSub_; + UMutex landmarksMutex_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; std::map imus_; std::string imuFrameId_; + UMutex imuMutex_; + rclcpp::Subscription::SharedPtr republishNodeDataSub_; rclcpp::Subscription::SharedPtr interOdomSub_; @@ -435,6 +508,23 @@ private: double localizationError_; }; LocalizationStatusTask localizationDiagnostic_; + + rclcpp::CallbackGroup::SharedPtr processingCallbackGroup_; + struct SyncData { + bool valid; + rclcpp::Time stamp; + rtabmap::SensorData data; + rtabmap::Transform odom; + std::vector odomVelocity; + std::string odomFrameId; + cv::Mat odomCovariance; + rtabmap::OdometryInfo odomInfo; + double timeMsgConversion; + }; + rclcpp::TimerBase::SharedPtr syncTimer_; + SyncData syncData_; + UMutex syncDataMutex_; + bool triggerNewMapBeforeNextUpdate_; }; } diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 4195a7c3..da30a527 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -2,7 +2,7 @@ rtabmap_slam - 0.21.5 + 0.22.0 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge geometry_msgs nav_msgs @@ -24,6 +26,10 @@ tf2 tf2_ros visualization_msgs + apriltag_msgs + aruco_msgs + aruco_opencv_msgs + rtabmap_msgs rtabmap_util diff --git a/rtabmap_slam/src/CoreNode.cpp b/rtabmap_slam/src/CoreNode.cpp index ec5d226a..d16aa4f1 100644 --- a/rtabmap_slam/src/CoreNode.cpp +++ b/rtabmap_slam/src/CoreNode.cpp @@ -84,8 +84,11 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); UINFO("rtabmap %s started...", RTABMAP_VERSION); - rclcpp::spin(std::make_shared(options)); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 89a7b01a..c214d91a 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 @@ -82,6 +90,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_conversions/MsgConversion.h" +#ifdef PRE_ROS_JAZZY +namespace rclcpp { + rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;} + rmw_qos_profile_t ParametersQoS() {return rmw_qos_profile_parameters;} +} +#endif + using namespace rtabmap; namespace rtabmap_slam { @@ -131,20 +146,18 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()), twoDMapping_(Parameters::defaultRegForce3DoF()), previousStamp_(0), - ulogToRosout_(this) + ulogToRosout_(this), + triggerNewMapBeforeNextUpdate_(false) { char * rosHomePath = getenv("ROS_HOME"); std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros"; databasePath_ = workingDir+"/"+rtabmap::Parameters::getDefaultDatabaseName(); - globalPose_.header.stamp = rclcpp::Time(0); mapsManager_.init(*this, this->get_name(), true); + syncData_.valid = false; + tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); @@ -196,6 +209,13 @@ 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."); + useActionForGoal_ = false; + } +#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_); @@ -230,6 +250,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance); RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "rtabmap: wait_for_transform = %f", waitForTransform_); if(this->isSubscribedToStereo()) { RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); @@ -256,6 +277,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(get_logger(), "rtabmap: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false"); } + // Create the processing timer + processingCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + syncTimer_ = this->create_wall_timer(0s, std::bind(&CoreWrapper::processAsync, this), processingCallbackGroup_); + syncTimer_->cancel(); + + rclcpp::SubscriptionOptions subOptions; + subOptions.callback_group = processingCallbackGroup_; + infoPub_ = this->create_publisher("info", 1); mapDataPub_ = this->create_publisher("mapData", 1); mapGraphPub_ = this->create_publisher("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); @@ -267,11 +296,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : localGridEmpty_ = this->create_publisher("local_grid_empty", 1); localGridGround_ = this->create_publisher("local_grid_ground", 1); localizationPosePub_ = this->create_publisher("localization_pose", 1); - initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1)); + initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1), subOptions); // planning topics - goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1)); - goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1)); + goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1), subOptions); + goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1), subOptions); nextMetricGoalPub_ = this->create_publisher("goal_out", 1); goalReachedPub_ = this->create_publisher("goal_reached", 1); globalPathPub_ = this->create_publisher("global_path", 1); @@ -323,7 +352,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } // declare parameters - this->declare_parameter("is_rtabmap_paused", paused_); + paused_ = this->declare_parameter("is_rtabmap_paused", paused_); if(paused_) { RCLCPP_WARN(get_logger(), "Node paused... don't forget to call service \"resume\" to start rtabmap."); @@ -335,7 +364,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : std::string vStr = this->declare_parameter(iter->first, iter->second); if(overrides.find(iter->first) != overrides.end()) { - RCLCPP_INFO(this->get_logger(), "Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + RCLCPP_INFO(this->get_logger(), "Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), vStr.c_str()); if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) { @@ -365,6 +394,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : char ** argv = new char*[argList.size()]; bool deleteDbOnStart = false; + deleteDbOnStart = this->declare_parameter("delete_db_on_start", deleteDbOnStart); for(unsigned int i=0; iget_logger(), "Subscribe to inter odom messages"); - interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1)); + interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1), subOptions); } } @@ -633,45 +663,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), rclcpp::ServicesQoS(), processingCallbackGroup_); + resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + triggerNewMapSrv_ = this->create_service(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + backupDatabase_ = this->create_service(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + detectMoreLoopClosuresSrv_ = this->create_service(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + globalBundleAdjustmentSrv_ = this->create_service(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + cleanupLocalGridsSrv_ = this->create_service(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setModeLocalizationSrv_ = this->create_service(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setModeMappingSrv_ = this->create_service(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getNodeDataSrv_ = this->create_service(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getMapDataSrv_ = this->create_service(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getMapData2Srv_ = this->create_service(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getMapSrv_ = this->create_service(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getProbMapSrv_ = this->create_service(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + publishMapDataSrv_ = this->create_service(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getPlanNodesSrv_ = this->create_service(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setGoalSrv_ = this->create_service(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + getNodesInRadiusSrv_ = this->create_service(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = this->create_service("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), rclcpp::ServicesQoS(), processingCallbackGroup_); + octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); #endif #endif //private services - setLogDebugSrv_ = this->create_service("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), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); + setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); @@ -684,19 +715,28 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rclcpp::Rate r(1.0 / tfDelay); while(tfThreadRunning_) { + mapToOdomMutex_.lock(); if(!odomFrameId_.empty()) { - mapToOdomMutex_.lock(); - rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance); geometry_msgs::msg::TransformStamped msg; + rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); msg.child_frame_id = odomFrameId_; msg.header.frame_id = mapFrameId_; - msg.header.stamp = tfExpiration; - rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); + msg.header.stamp = now() + rclcpp::Duration::from_seconds(tfTolerance); tfBroadcaster_->sendTransform(msg); - mapToOdomMutex_.unlock(); } - r.sleep(); + mapToOdomMutex_.unlock(); + try { + r.sleep(); + } + catch(std::exception & e) { + if(rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), + "Could not sleep: \"%s\", TF \"%s\"->\"%s\" won't be published anymore!", + e.what(), mapFrameId_.c_str(), odomFrameId_.c_str()); + } // else: the node may have been shutdown + break; + } } }); } @@ -730,9 +770,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } - auto node = rclcpp::Node::make_shared("rtabmap"); image_transport::TransportHints hints(this); - defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile()); + defaultSub_ = image_transport::create_subscription(this, "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); @@ -818,25 +857,55 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } this->set_parameters(rosParameters); - int qosGPS = 0; - int qosIMU = 0; + // Setup callback groups for any subscriptions that should not be affected by main processing thread. + userDataAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + globalPoseAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions userDataAsyncSubOptions; + rclcpp::SubscriptionOptions globalPoseAsyncSubOptions; + rclcpp::SubscriptionOptions gpsAsyncSubOptions; + rclcpp::SubscriptionOptions landmarkSubOptions; + rclcpp::SubscriptionOptions imuSubOptions; + userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_; + globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_; + gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_; + landmarkSubOptions.callback_group = imuCallbackGroup_; + imuSubOptions.callback_group = imuCallbackGroup_; + + int qosGPS = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + int qosIMU = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); - userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1)); - globalPoseAsyncSub_ = this->create_subscription("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1)); - gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionSub_ = this->create_subscription("landmark_detection", 5, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 5, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1)); + userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions); + globalPoseAsyncSub_ = this->create_subscription("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions); + gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions); + landmarkDetectionSub_ = this->create_subscription("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS - tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1)); + tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + apriltagSub_ = this->create_subscription("apriltag/detections", 5, std::bind(&CoreWrapper::apriltagAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MSGS + arucoSub_ = this->create_subscription("aruco/detections", 5, std::bind(&CoreWrapper::arucoAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + arucoOpencvSub_ = this->create_subscription("aruco_opencv/detections", 5, std::bind(&CoreWrapper::arucoOpencvAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + arucoMarkersSub_ = this->create_subscription("aruco_markers/detections", 5, std::bind(&CoreWrapper::arucoMarkersAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + arucoInterfacesSub_ = this->create_subscription("aruco_interfaces/detections", 5, std::bind(&CoreWrapper::arucoInterfacesAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS - fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1)); + fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif - imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1)); - republishNodeDataSub_ = this->create_subscription("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1)); + imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1), imuSubOptions); + republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 1, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1), subOptions); + parametersClient_ = std::make_shared(this, std::string(), rclcpp::ParametersQoS(), processingCallbackGroup_); - parametersClient_ = std::make_shared(this); auto on_parameter_event_callback = [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void { @@ -887,12 +956,20 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "RTAB-Map rate detection = %f Hz", rate_); } rtabmap_.parseParameters(parameters_); - mapsManager_.setParameters(parameters_); + // Don't reset map in localization mode + if(rtabmap_.getMemory()->isIncremental()) { + mapsManager_.setParameters(parameters_); + } } }; // Setup callback for changes to parameters. - parameterEventSub_ = parametersClient_->on_parameter_event(on_parameter_event_callback); + rclcpp::SubscriptionOptionsWithAllocator> paramOptions; + paramOptions.callback_group = processingCallbackGroup_; + parameterEventSub_ = parametersClient_->on_parameter_event( + on_parameter_event_callback, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)), + paramOptions); } CoreWrapper::~CoreWrapper() @@ -1055,11 +1132,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti } } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg.pose.covariance[0] >= BAD_COVARIANCE && odomMsg.twist.covariance[0] >= BAD_COVARIANCE))) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0])); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1094,9 +1173,9 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti covariance.at(0,0)>0.0) { // Use largest covariance error (to be independent of the odometry frame rate) - if(covariance_.empty() || covariance.at(0,0) > covariance_.at(0,0)) + if(lastPoseCovariance_.empty() || covariance.at(0,0) > lastPoseCovariance_.at(0,0)) { - covariance_ = covariance; + lastPoseCovariance_ = covariance; } } } @@ -1126,32 +1205,30 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } return false; } -bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) +bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp) { if(!paused_) { // Odom TF ready? - Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, *tfBuffer_, waitForTransform_); + Transform odom = rtabmap_conversions::getTransform(odomFrameId, frameId_, stamp, *tfBuffer_, waitForTransform_); if(odom.isNull()) { return false; } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && odom.isIdentity()) { UWARN("Odometry is reset (identity pose detected). Increment map id!"); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1183,10 +1260,6 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } @@ -1208,7 +1281,7 @@ void CoreWrapper::commonMultiCameraCallback( const std::vector > & localPoints3d, const std::vector & localDescriptors) { - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1231,38 +1304,53 @@ void CoreWrapper::commonMultiCameraCallback( return; } } - else if(!scan2dMsg.ranges.empty()) + else { - if(!odomTFUpdate(scan2dMsg.header.stamp)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) + { + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } + } + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(odomFrameId, cameraInfoMsgs[0].header.stamp)) { return; } } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) - { - return; - } - } - else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp)) - { - return; - } - commonMultiCameraCallbackImpl(odomFrameId, - userDataMsg, - imageMsgs, - depthMsgs, - cameraInfoMsgs, - depthCameraInfoMsgs, - scan2dMsg, - scan3dMsg, - odomInfoMsg, - globalDescriptorMsgs, - localKeyPoints, - localPoints3d, - localDescriptors); + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) + { + UScopeMutex lock(lastPoseMutex_); + commonMultiCameraCallbackImpl(odomFrameId, + userDataMsg, + imageMsgs, + depthMsgs, + cameraInfoMsgs, + depthCameraInfoMsgs, + scan2dMsg, + scan3dMsg, + odomInfoMsg, + globalDescriptorMsgs, + localKeyPoints, + localPoints3d, + localDescriptors); + + if(syncData_.valid) { + syncTimer_->reset(); + } + syncDataMutex_.unlock(); + } } void CoreWrapper::commonMultiCameraCallbackImpl( @@ -1512,10 +1600,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl( userData_ = cv::Mat(); } - SensorData data; if(!stereoCameraModels.empty()) { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1526,7 +1613,7 @@ void CoreWrapper::commonMultiCameraCallbackImpl( } else { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1544,25 +1631,31 @@ void CoreWrapper::commonMultiCameraCallbackImpl( if(!globalDescriptorMsgs.empty()) { - data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); + syncData_.data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); } if(!keypoints.empty()) { UASSERT(points.empty() || points.size() == keypoints.size()); UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size()); - data.setFeatures(keypoints, points, descriptors); + syncData_.data.setFeatures(keypoints, points, descriptors); } - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - covariance_ = cv::Mat(); + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); } void CoreWrapper::commonLaserScanCallback( @@ -1574,7 +1667,7 @@ void CoreWrapper::commonLaserScanCallback( const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor) { UTimer timerConversion; - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1597,110 +1690,128 @@ void CoreWrapper::commonLaserScanCallback( return; } } - else if(!scan2dMsg.ranges.empty()) - { - if(!odomTFUpdate(scan2dMsg.header.stamp)) - { - return; - } - } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) - { - return; - } - } else { - return; - } - - LaserScan scan; - if(!scan2dMsg.ranges.empty()) - { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - // backward compatibility, project 2D scan in /base_link frame - rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); - return; + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - scanCloudMaxPoints_, - 0, - scanCloudIs2d_)) + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); return; } } - cv::Mat userData; - if(userDataMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + UScopeMutex lock(lastPoseMutex_); + LaserScan scan; + if(!scan2dMsg.ranges.empty()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + if(!rtabmap_conversions::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + // backward compatibility, project 2D scan in /base_link frame + rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); + return; + } + } + else if(!scan3dMsg.data.empty()) + { + if(!rtabmap_conversions::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + scanCloudMaxPoints_, + 0, + scanCloudIs2d_)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); + return; + } + } + + cv::Mat userData; + if(userDataMsg.get()) + { + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } + + syncData_.data = SensorData( + scan, + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + if(!globalDescriptor.data.empty()) + { + syncData_.data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - - SensorData data( - scan, - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - - if(!globalDescriptor.data.empty()) - { - data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); - } - - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); } void CoreWrapper::commonOdomCallback( @@ -1710,56 +1821,66 @@ void CoreWrapper::commonOdomCallback( { UTimer timerConversion; UASSERT(odomMsg.get()); - std::string odomFrameId = odomFrameId_; - - odomFrameId = odomMsg->header.frame_id; + std::string odomFrameId = odomMsg->header.frame_id; if(!odomUpdate(*odomMsg, odomMsg->header.stamp)) { return; } - cv::Mat userData; - if(userDataMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + UScopeMutex lock(lastPoseMutex_); + cv::Mat userData; + if(userDataMsg.get()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } + + syncData_.data = SensorData( + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - - SensorData data( - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); } void CoreWrapper::commonSensorDataCallback( @@ -1769,7 +1890,7 @@ void CoreWrapper::commonSensorDataCallback( { UTimer timerConversion; UASSERT(sensorDataMsg.get()); - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1778,30 +1899,73 @@ void CoreWrapper::commonSensorDataCallback( return; } } - else if(!odomTFUpdate(sensorDataMsg->header.stamp)) + else { - return; + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) + { + return; + } } - SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); - data.setId(lastPoseIntermediate_?-1:0); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); + UScopeMutex lock(lastPoseMutex_); + syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); + syncData_.data.setId(lastPoseIntermediate_?-1:0); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); + } +} + +void CoreWrapper::processAsync() +{ + UScopeMutex lock(syncDataMutex_); + + if(triggerNewMapBeforeNextUpdate_) + { + rtabmap_.triggerNewMap(); + triggerNewMapBeforeNextUpdate_ = false; } - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - - covariance_ = cv::Mat(); + if(syncData_.valid) + { + process(syncData_.stamp, + syncData_.data, + syncData_.odom, + syncData_.odomVelocity, + syncData_.odomFrameId, + syncData_.odomCovariance, + syncData_.odomInfo, + syncData_.timeMsgConversion); + syncData_.valid=false; + } + syncTimer_->cancel(); } void CoreWrapper::process( @@ -1820,7 +1984,7 @@ void CoreWrapper::process( // Add intermediate nodes? for(std::list >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) { - if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < lastPoseStamp_) + if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp) { Transform interOdom; if(!rtabmap_.getLocalOptimizedPoses().empty()) @@ -1909,7 +2073,7 @@ void CoreWrapper::process( } interOdoms_.erase(iter++); } - else if(iter->first.header.stamp == lastPoseStamp_) + else if(iter->first.header.stamp == stamp) { interOdoms_.erase(iter++); break; @@ -1924,31 +2088,53 @@ void CoreWrapper::process( Transform groundTruthPose; if(!groundTruthFrameId_.empty()) { - groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, *tfBuffer_, waitForTransform_); + groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp, *tfBuffer_, waitForTransform_); } data.setGroundTruth(groundTruthPose); //global pose - if(globalPose_.header.stamp.sec != 0 || globalPose_.header.stamp.nanosec != 0) + geometry_msgs::msg::PoseWithCovarianceStamped globalPoseMsg; + globalPoseMsg.header.stamp = rclcpp::Time(0); + { + UScopeMutex lock(globalPoseMutex_); + if(!globalPoses_.empty()) + { + auto iter = rtabmap_conversions::getClosestIterator(globalPoses_, data.stamp()); + // Check if it is not too old + if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_) + { + globalPoseMsg = iter->second; + } + else + { + RCLCPP_WARN(this->get_logger(), "Ignoring global pose with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).", + iter->first, + 1.0/rate_, + data.stamp()); + } + globalPoses_.clear(); + } + } + if(globalPoseMsg.header.stamp.sec != 0 || globalPoseMsg.header.stamp.nanosec != 0) { // assume sensor is fixed Transform sensorToBase = rtabmap_conversions::getTransform( - globalPose_.header.frame_id, + globalPoseMsg.header.frame_id, frameId_, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_); if(!sensorToBase.isNull()) { - Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPose_.pose.pose); + Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPoseMsg.pose.pose); globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame // Correction of the global pose accounting the odometry movement since we received it Transform correction = rtabmap_conversions::getMovingTransform( frameId_, odomFrameId, - lastPoseStamp_, - rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec), + stamp, + rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec), *tfBuffer_, waitForTransform_); if(!correction.isNull()) @@ -1961,40 +2147,59 @@ void CoreWrapper::process( "If odometry is small since it received the global pose and " "covariance is large, this should not be a problem."); } - cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone(); + cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPoseMsg.pose.covariance.data()).clone(); data.setGlobalPose(globalPose, globalPoseCovariance); } } - globalPose_.header.stamp = rclcpp::Time(0); - if(gps_.stamp() > 0.0) { - data.setGPS(gps_); + UScopeMutex lock(gpsMutex_); + if(!gps_.empty()) + { + std::map::const_iterator iter = rtabmap_conversions::getClosestIterator(gps_, data.stamp()); + // Check if it is not too old + if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_) + { + data.setGPS(iter->second); + } + else + { + RCLCPP_WARN(this->get_logger(), "Ignoring GPS with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).", + iter->first, + 1.0/rate_, + data.stamp()); + } + gps_.clear(); + } } - gps_ = rtabmap::GPS(); + //tag detections + landmarksMutex_.lock(); Landmarks landmarks = rtabmap_conversions::landmarksFromROS( landmarks_, frameId_, odomFrameId, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_, landmarkDefaultLinVariance_, landmarkDefaultAngVariance_); landmarks_.clear(); + landmarksMutex_.unlock(); if(!landmarks.empty()) { data.setLandmarks(landmarks); } // IMU + imuMutex_.lock(); if(!imus_.empty()) { Transform t = Transform::getTransform(imus_, data.stamp()); if(!t.isNull()) { + imuMutex_.unlock(); // get local transform rtabmap::Transform localTransform; if(frameId_.compare(imuFrameId_) != 0) @@ -2018,10 +2223,15 @@ void CoreWrapper::process( else { RCLCPP_WARN(this->get_logger(), "We are receiving imu data (buffer=%d), but cannot interpolate " - "imu transform at time %f. IMU won't be added to graph.", - (int)imus_.size(), data.stamp()); + "imu transform at time %f (latest imu received with stamp %f). IMU won't be added to graph.", + (int)imus_.size(), data.stamp(), imus_.rbegin()->first); + imuMutex_.unlock(); } } + else + { + imuMutex_.unlock(); + } double timeRtabmap = 0.0; double timeUpdateMaps = 0.0; @@ -2087,7 +2297,7 @@ void CoreWrapper::process( timeRtabmap = timer.ticks(); mapToOdomMutex_.lock(); mapToOdom_ = rtabmap_.getMapCorrection(); - + Transform mapToOdomSafe = mapToOdom_.clone(); if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0) { RCLCPP_ERROR(get_logger(), "Odometry received doesn't have same frame_id " @@ -2116,7 +2326,7 @@ void CoreWrapper::process( geometry_msgs::msg::PoseWithCovarianceStamped poseMsg; poseMsg.header.frame_id = mapFrameId_; poseMsg.header.stamp = stamp; - rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); + rtabmap_conversions::transformToPoseMsg(mapToOdomSafe*odom, poseMsg.pose.pose); if(!rtabmap_.getStatistics().localizationCovariance().empty()) { const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); @@ -2149,12 +2359,12 @@ void CoreWrapper::process( SensorData tmpData = data; tmpData.setId(0); tmpSignature.insert(std::make_pair(0, Signature(0, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); - filteredPoses.insert(std::make_pair(0, mapToOdom_*odom)); + filteredPoses.insert(std::make_pair(0, mapToOdomSafe*odom)); } if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1) { - std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdom_*odom); + std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdomSafe*odom); //add latest/zero and make sure those on a planned path are not filtered std::set onPath; if(rtabmap_.getPath().size()) @@ -2199,7 +2409,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 +2423,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()) @@ -2294,7 +2510,7 @@ void CoreWrapper::process( { timeRtabmap = timer.ticks(); } - RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", + RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs delay=%.4fs (local map=%d, WM=%d)", rtabmap_.getLastLocationId(), rate_>0?1.0f/rate_:0, rtabmap_.getTimeThreshold()/1000.0f, @@ -2302,6 +2518,7 @@ void CoreWrapper::process( timeRtabmap, timeUpdateMaps, timePublishMaps, + (now() - stamp).seconds(), (int)rtabmap_.getLocalOptimizedPoses().size(), rtabmap_.getWMSize()+rtabmap_.getSTMSize()); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0)); @@ -2374,7 +2591,12 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCova { if(!paused_) { - globalPose_ = *globalPoseMsg; + UScopeMutex lock(globalPoseMutex_); + globalPoses_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(globalPoseMsg->header.stamp), *globalPoseMsg)); + if(globalPoses_.size() > 1000) + { + globalPoses_.erase(globalPoses_.begin()); + } } } @@ -2391,13 +2613,21 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP error = sqrt(variance); } } - gps_ = rtabmap::GPS( - rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp), - gpsFixMsg->longitude, - gpsFixMsg->latitude, - gpsFixMsg->altitude, - error, - 0); + + rtabmap::GPS gps( + rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp), + gpsFixMsg->longitude, + gpsFixMsg->latitude, + gpsFixMsg->altitude, + error, + 0); + + UScopeMutex lock(gpsMutex_); + gps_.insert(std::make_pair(gps.stamp(), gps)); + if(gps_.size() > 1000) + { + gps_.erase(gps_.begin()); + } } } @@ -2408,6 +2638,7 @@ void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::Landma geometry_msgs::msg::PoseWithCovarianceStamped p; p.header = landmarkDetection->header; p.pose = landmarkDetection->pose; + UScopeMutex lock(landmarksMutex_); uInsert(landmarks_, std::make_pair(landmarkDetection->id, std::make_pair(p, landmarkDetection->size))); @@ -2418,6 +2649,7 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; ilandmarks.size(); ++i) { geometry_msgs::msg::PoseWithCovarianceStamped p; @@ -2431,17 +2663,30 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm } #ifdef WITH_APRILTAG_MSGS -void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections) +void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) { if(!paused_) { - for(unsigned int i=0; idetections.size(); ++i) + static bool warningShow = false; + if(!warningShow) { + RCLCPP_WARN(this->get_logger(), "\"tag_detections\" input topic name for apriltag_msgs is deprecated, remap \"apriltag\" input topic name instead. This message is only printed once."); + warningShow = true; + } + apriltagAsyncCallback(msg); + } +} +void CoreWrapper::apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; idetections.size(); ++i) { - std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id); + std::string tagFrameId = msg->detections[i].family+":"+uNumber2Str(msg->detections[i].id); Transform camToTag = rtabmap_conversions::getTransform( - tagDetections->header.frame_id, // e.g., camera_optical_frame + msg->header.frame_id, // e.g., camera_optical_frame tagFrameId, // e.g., tag36h11:42 - tagDetections->header.stamp, + msg->header.stamp, *tfBuffer_, waitForTransform_); if(camToTag.isNull()) @@ -2449,16 +2694,97 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.", frameId_.c_str(), tagFrameId.c_str(), - tagDetections->detections[i].id); + msg->detections[i].id); continue; } geometry_msgs::msg::PoseWithCovarianceStamped p; rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose); - p.header = tagDetections->header; + p.header = msg->header; uInsert(landmarks_, - std::make_pair(tagDetections->detections[i].id, + std::make_pair(msg->detections[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MSGS +void CoreWrapper::arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose = msg->markers[i].pose; + p.header = msg->markers[i].header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +void CoreWrapper::arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].marker_id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +void CoreWrapper::arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose.pose; + p.header = msg->markers[i].pose.header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +void CoreWrapper::arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + UASSERT(msg->marker_ids.size() == msg->poses.size()); + for(unsigned int i=0; imarker_ids.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->poses[i]; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->marker_ids[i], std::make_pair(p, 0.0f))); } } @@ -2470,6 +2796,7 @@ void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::Fidu { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; iorientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); imus_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(msg->header.stamp), orientation)); if(imus_.size() > 1000) @@ -2545,14 +2873,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::msg::Odometry::ConstShar void CoreWrapper::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose); - if(intialPose.isNull()) + Transform mapToPose = Transform::getIdentity(); + if(msg->header.frame_id.empty()) { - RCLCPP_ERROR(this->get_logger(), "Pose received is null!"); - return; + RCLCPP_WARN(this->get_logger(), "Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str()); + } + else if(msg->header.frame_id != mapFrameId_) + { + mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_); + if(mapToPose.isNull()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str()); + return; + } } - rtabmap_.setInitialPose(intialPose); + Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose); + if(initialPose.isNull()) + { + RCLCPP_ERROR(this->get_logger(), "initialpose received is null!"); + return; + } + if(mapToPose.isIdentity()) + { + RCLCPP_INFO(this->get_logger(), "initialpose received: %s", initialPose.prettyPrint().c_str()); + rtabmap_.setInitialPose(initialPose); + } + else + { + RCLCPP_INFO(this->get_logger(), "initialpose received: %s in %s frame, transformed to %s in %s frame.", + initialPose.prettyPrint().c_str(), + msg->header.frame_id.c_str(), + (mapToPose * initialPose).prettyPrint().c_str(), + mapFrameId_.c_str()); + rtabmap_.setInitialPose(mapToPose*initialPose); + } } void CoreWrapper::goalCommonCallback( @@ -2796,7 +3151,10 @@ void CoreWrapper::updateRtabmapCallback( RCLCPP_INFO(get_logger(), "2D mapping = %s", twoDMapping_?"true":"false"); } rtabmap_.parseParameters(parameters_); - mapsManager_.setParameters(parameters_); + // Don't reset map in localization mode + if(rtabmap_.getMemory()->isIncremental()) { + mapsManager_.setParameters(parameters_); + } } void CoreWrapper::resetRtabmapCallback( @@ -2806,10 +3164,15 @@ void CoreWrapper::resetRtabmapCallback( { RCLCPP_INFO(this->get_logger(), "rtabmap: Reset"); rtabmap_.resetMemory(); - covariance_ = cv::Mat(); + + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2817,14 +3180,18 @@ void CoreWrapper::resetRtabmapCallback( graphLatched_ = false; mapsManager_.clear(); previousStamp_ = rclcpp::Time(0); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -2900,10 +3267,14 @@ void CoreWrapper::loadDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2911,14 +3282,18 @@ void CoreWrapper::loadDatabaseCallback( graphLatched_ = false; mapsManager_.clear(); previousStamp_ = rclcpp::Time(0); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -2927,14 +3302,16 @@ void CoreWrapper::loadDatabaseCallback( // Open new database databasePath_ = newDatabasePath; - // modify default parameters with those in the database + //Warn if database's parameters are different than the current ones we are using if(!req->clear && UFile::exists(databasePath_)) { ParametersMap dbParameters; rtabmap::DBDriver * driver = rtabmap::DBDriver::create(); + std::string databaseVersion = "0.0.0"; if(driver->openConnection(databasePath_)) { dbParameters = driver->getLastParameters(); // parameter migration is already done + databaseVersion = driver->getDatabaseVersion(); } delete driver; for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter) @@ -2944,15 +3321,31 @@ void CoreWrapper::loadDatabaseCallback( // ignore working directory continue; } - if(parameters_.find(iter->first) == parameters_.end() && - parameters_.find(iter->first)->second.compare(iter->second) !=0) + if(iter->first.find("Odom") == 0) { - RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s) is different " - "from the current used one (%s). We still keep the " + // ignore odometry params + continue; + } + if(parameters_.find(iter->first) == parameters_.end()) + { + RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s, version \"%s\") doesn't exist " + "in current rtabmap version (\"%s\"). The parameter is ignored.", + iter->first.c_str(), + iter->second.c_str(), + databaseVersion.c_str(), + RTABMAP_VERSION); + } + else if(parameters_.find(iter->first)->second.compare(iter->second) !=0) + { + RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s, version=\"%s\") is different " + "from the current used one (%s, version=\"%s\"). We still keep the " "current parameter value (%s). If you want to switch between databases " "with different configurations, restart rtabmap node instead of using this service.", - iter->first.c_str(), iter->second.c_str(), + iter->first.c_str(), + iter->second.c_str(), + databaseVersion.c_str(), parameters_.find(iter->first)->second.c_str(), + RTABMAP_VERSION, parameters_.find(iter->first)->second.c_str()); } } @@ -3027,9 +3420,14 @@ void CoreWrapper::backupDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(this->get_logger(), "Backup: Saving memory... done!"); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); + lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -3038,9 +3436,11 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); RCLCPP_INFO(this->get_logger(), "Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back"); @@ -3365,11 +3765,15 @@ void CoreWrapper::getMapDataCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3409,11 +3813,15 @@ void CoreWrapper::getMapData2Callback( req->with_words, req->with_global_descriptors); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3532,6 +3940,10 @@ void CoreWrapper::publishMapCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + if(mapDataPub_->get_subscription_count()) { rtabmap_msgs::msg::MapData::UniquePtr msg(new rtabmap_msgs::msg::MapData); @@ -3541,7 +3953,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, *msg); mapDataPub_->publish(std::move(msg)); @@ -3555,7 +3967,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapGraphToROS(poses, constraints, - mapToOdom_, + mapToOdomSafe, *msg); mapGraphPub_->publish(std::move(msg)); @@ -3964,11 +4376,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 +4777,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,24 +4809,23 @@ 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_; } } } - +#ifdef WITH_NAV2_MSGS void CoreWrapper::goalResponseCallback( #ifdef NAV_MSGS_FOXY std::shared_future future) { auto goal_handle = future.get(); #else - const GoalHandleNav2::SharedPtr & goal_handle) + const GoalHandleNav2::SharedPtr & goal_handle) { #endif if (!goal_handle) { @@ -4424,6 +4837,7 @@ void CoreWrapper::goalResponseCallback( latestNodeWasReached_ = false; } else { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + lastGoalSent_ = goal_handle->get_goal_id(); } } @@ -4449,6 +4863,11 @@ void CoreWrapper::resultCallback( RCLCPP_INFO(this->get_logger(), "Planning: nav2 success!"); } } + else if(result.code==rclcpp_action::ResultCode::ABORTED && result.goal_id != lastGoalSent_) + { + // Just ignored, it is from an old goal + ignore = true; + } else { RCLCPP_ERROR(this->get_logger(), "Planning: nav2 failed for some reason: %s. Aborting the plan...", @@ -4473,6 +4892,7 @@ void CoreWrapper::resultCallback( latestNodeWasReached_ = false; } } +#endif void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp) { diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 6b45dcd7..5bbf39fd 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -1,6 +1,20 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_sync) +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + crypto_LIB NAMES crypto + PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index a2e9c681..1f3f63b2 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -29,15 +29,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include +#ifdef PRE_ROS_IRON #include +#else +#include +#endif #include #include @@ -74,7 +78,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_;} @@ -112,147 +117,135 @@ protected: const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0; - void commonSingleCameraCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr & imageMsg, - const cv_bridge::CvImageConstPtr & depthMsg, - const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, - const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, - const sensor_msgs::msg::LaserScan & scanMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector & globalDescriptorMsgs = std::vector(), - const std::vector & localKeyPoints = std::vector(), - const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); - void tick(const rclcpp::Time & stamp, double targetFrequency = 0); private: + void commonSingleCameraCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const cv_bridge::CvImageConstPtr & imageMsg, + const cv_bridge::CvImageConstPtr & depthMsg, + const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, + const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs = std::vector(), + const std::vector & localKeyPoints = std::vector(), + const std::vector & localPoints3d = std::vector(), + const cv::Mat & localDescriptors = cv::Mat()); + void processSyncData(); void setupDepthCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupStereoCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDXCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #ifdef RTABMAP_SYNC_MULTI_RGBD void setupRGBD2Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD3Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD4Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD5Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD6Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #endif void setupSensorDataCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupScanCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeScan2d, bool subscribeScanDesc, bool subscribeOdom, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupOdomCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); protected: std::string subscribedTopicsMsg_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; rmw_qos_reliability_policy_t qosOdom_; rmw_qos_reliability_policy_t qosImage_; rmw_qos_reliability_policy_t qosCameraInfo_; @@ -277,6 +270,8 @@ private: int rgbdCameras_; std::string name_; + rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; + //for depth and rgb-only callbacks image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageDepthSub_; diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h index 48de6514..609af84b 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h @@ -181,7 +181,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", \ getTopicName(SUB0.getSubscriber()).c_str(), \ getTopicName(SUB1.getSubscriber()).c_str(), \ getTopicName(SUB2.getSubscriber()).c_str(), \ diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 557f80b6..03b49348 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -8,6 +8,7 @@ #include "rtabmap_conversions/MsgConversion.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMutex.h" using namespace std::chrono_literals; @@ -15,15 +16,20 @@ 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) + SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) : + node_(node), + diagnosticUpdater_(node, 2.0), + inFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&inTargetFrequency_, &inTargetFrequency_, tolerance), node->get_clock()), + inTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), + outFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&outTargetFrequency_, &outTargetFrequency_, tolerance), node->get_clock()), + outTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()), + inCompositeTask_("Input Status"), + outCompositeTask_("Output Status"), + lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), + inTargetFrequency_(0.0), + outTargetFrequency_(0.0), + windowSize_(windowSize), + lastTickTime_(0.0) { UASSERT(windowSize_ >= 1); } @@ -41,71 +47,133 @@ class SyncDiagnostic { // Assuming format is /back_camera/left/image, we want "back_camera" strList.pop_back(); } - compositeTask_.addTask(&frequencyStatus_); - compositeTask_.addTask(&timeStampStatus_); - diagnosticUpdater_.add(compositeTask_); + inCompositeTask_.addTask(&inFrequencyStatus_); + inCompositeTask_.addTask(&inTimeStampStatus_); + diagnosticUpdater_.add(inCompositeTask_); + outCompositeTask_.addTask(&outFrequencyStatus_); + outCompositeTask_.addTask(&outTimeStampStatus_); + diagnosticUpdater_.add(outCompositeTask_); for(size_t i=0; icreate_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); + diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } - void tick(const rclcpp::Time & stamp, double targetFrequency = 0) + void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0) { - frequencyStatus_.tick(); - timeStampStatus_.tick(stamp); - double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_; + updateFrequency( + stamp, + expectedFrequency, + inFrequencyStatus_, + inTimeStampStatus_, + inWindow_, + inTargetFrequency_, + lastTickInputStamp_); + } - window_.push_back(singlePeriod); - if(window_.size() > windowSize_) - { - window_.pop_front(); - } - double period = 0.0; - if(window_.size() == windowSize_) - { - for(size_t i=0; i0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_)) - { - targetFrequency_ = 1.0/period; - } - else if(targetFrequency>0) - { - targetFrequency_ = targetFrequency; - } - lastCallbackCalledStamp_ = rtabmap_conversions::timestampFromROS(stamp); + void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) + { + double lastTickOutputStamp; + updateFrequency( + stamp, + expectedFrequency, + outFrequencyStatus_, + outTimeStampStatus_, + outWindow_, + outTargetFrequency_, + lastTickOutputStamp); } private: void diagnosticTimerCallback() { - if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) + UScopeMutex lock(tickMutex_); + if(rtabmap_conversions::timestampFromROS(node_->now())-lastTickInputStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", topicsNotReceivedWarningMsg_.c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", topicsNotReceivedWarningMsg_.c_str()); } } + void updateFrequency( + const rclcpp::Time & stamp, + const double & expectedFrequency, + diagnostic_updater::FrequencyStatus & freqStatus, + diagnostic_updater::TimeStampStatus & timeStatus, + std::deque & window, + double & targetFrequency, + double & lastTickStamp) + { + UScopeMutex lock(tickMutex_); + + freqStatus.tick(); + timeStatus.tick(stamp); + + double stampSec = rtabmap_conversions::timestampFromROS(stamp); + double singlePeriod = stampSec - lastTickStamp; + + window.push_back(singlePeriod); + if(window.size() > windowSize_) + { + window.pop_front(); + + double period = 0.0; + if(window.size() == windowSize_) + { + for(size_t i=0; i0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) + { + targetFrequency = 1.0/period; + } + else if(expectedFrequency>0) + { + targetFrequency = expectedFrequency; + + } + } + + lastTickStamp = stampSec; + + double clockNow = rtabmap_conversions::timestampFromROS(node_->now()); + if(lastTickTime_ > clockNow) + { + RCLCPP_WARN(node_->get_logger(), "%s: Detected time jump in the past of %f sec, forcing diagnostic update.", + node_->get_name(), lastTickTime_ - clockNow); + inFrequencyStatus_.clear(); + outFrequencyStatus_.clear(); + diagnosticUpdater_.force_update(); + lastTickInputStamp_ = clockNow; + } + lastTickTime_ = clockNow; + } + private: rclcpp::Node * node_; std::string topicsNotReceivedWarningMsg_; diagnostic_updater::Updater diagnosticUpdater_; - diagnostic_updater::FrequencyStatus frequencyStatus_; - diagnostic_updater::TimeStampStatus timeStampStatus_; - diagnostic_updater::CompositeDiagnosticTask compositeTask_; + diagnostic_updater::FrequencyStatus inFrequencyStatus_; + diagnostic_updater::TimeStampStatus inTimeStampStatus_; + diagnostic_updater::FrequencyStatus outFrequencyStatus_; + diagnostic_updater::TimeStampStatus outTimeStampStatus_; + diagnostic_updater::CompositeDiagnosticTask inCompositeTask_; + diagnostic_updater::CompositeDiagnosticTask outCompositeTask_; rclcpp::TimerBase::SharedPtr diagnosticTimer_; - double lastCallbackCalledStamp_; - double targetFrequency_; + double lastTickInputStamp_; + double inTargetFrequency_; + double outTargetFrequency_; int windowSize_; - std::deque window_; + std::deque inWindow_; + std::deque outWindow_; + UMutex tickMutex_; + double lastTickTime_; }; diff --git a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp index cdbcae25..429cac57 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp @@ -61,6 +61,7 @@ private: double depthScale_; int decimation_; double compressedRate_; + double approxSyncMaxInterval_; rclcpp::Time lastCompressedPublished_; diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index fb1cde35..769155cc 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -2,7 +2,7 @@ rtabmap_sync - 0.21.5 + 0.22.0 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_transport message_filters diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index d28fe1d3..d1370c8e 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -31,7 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : - queueSize_(10), + topicQueueSize_(10), + syncQueueSize_(10), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -362,6 +363,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : { name_ = node.get_name(); + syncCallbackGroup_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // ROS related parameters (private) // ros2: should be declared in the constructor to be used by inherited classes in their constructor subscribedToDepth_ = node.declare_parameter("subscribe_depth", subscribedToDepth_); @@ -378,9 +381,19 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : odomFrameId_ = node.declare_parameter("odom_frame_id", odomFrameId_); rgbdCameras_ = node.declare_parameter("rgbd_cameras", rgbdCameras_); - queueSize_ = node.declare_parameter("queue_size", queueSize_); + topicQueueSize_ = node.declare_parameter("topic_queue_size", topicQueueSize_); + int queueSize = node.declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize_ = queueSize; + RCLCPP_WARN(node.get_logger(), "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_); + } + syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_); - int qos = node.declare_parameter("qos", 0); + int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT); int qosOdom = node.declare_parameter("qos_odom", qos); int qosImage = node.declare_parameter("qos_image", qos); int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage); @@ -519,7 +532,8 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_cloud = %s", name_.c_str(), subscribedToScan3d_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_descriptor = %s", name_.c_str(), subscribedToScanDescriptor_?"true":"false"); - RCLCPP_INFO(node.get_logger(), "%s: queue_size = %d", name_.c_str(), queueSize_); + RCLCPP_INFO(node.get_logger(), "%s: topic_queue_size = %d", name_.c_str(), topicQueueSize_); + RCLCPP_INFO(node.get_logger(), "%s: sync_queue_size = %d", name_.c_str(), syncQueueSize_); RCLCPP_INFO(node.get_logger(), "%s: qos_image = %d", name_.c_str(), qosImage_); RCLCPP_INFO(node.get_logger(), "%s: qos_camera_info = %d", name_.c_str(), qosCameraInfo_); RCLCPP_INFO(node.get_logger(), "%s: qos_scan = %d", name_.c_str(), qosScan_); @@ -527,41 +541,41 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_); RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false"); + rclcpp::SubscriptionOptions callbackOptions; + callbackOptions.callback_group = syncCallbackGroup_; + subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_; if(subscribedToDepth_) { setupDepthCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(subscribedToStereo_) { setupStereoCallbacks( node, + callbackOptions, subscribedToOdom_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(subscribedToRGB_) { setupRGBCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(subscribedToRGBD_) { @@ -577,66 +591,61 @@ void CommonDataSubscriber::setupCallbacks( setupRGBD6Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(rgbdCameras_ == 5) { setupRGBD5Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(rgbdCameras_ == 4) { setupRGBD4Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(rgbdCameras_ == 3) { setupRGBD3Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(rgbdCameras_ == 2) { setupRGBD2Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } #else if(rgbdCameras_>1) @@ -651,58 +660,53 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBDXCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else { setupRGBDCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, subscribedToScan3d_, subscribedToScanDescriptor_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } } else if(subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_) { setupScanCallbacks( node, + callbackOptions, subscribedToScan2d_, subscribedToScanDescriptor_, subscribedToOdom_, subscribedToUserData_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(subscribedToSensorData_) { setupSensorDataCallbacks( node, + callbackOptions, subscribedToOdom_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } else if(subscribedToOdom_) { setupOdomCallbacks( node, + callbackOptions, subscribedToUserData_, - subscribedToOdomInfo_, - queueSize_, - approxSync_); + subscribedToOdomInfo_); } if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_) @@ -713,10 +717,14 @@ void CommonDataSubscriber::setupCallbacks( uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " "header are set. If topics are coming from different computers, make sure " - "the clocks of the computers are synchronized (\"ntpdate\"). %s%s", + "the clocks of the computers are synchronized (\"ntpdate\"). Ajusting " + "topic_queue_size (%d) and sync_queue_size (%d) can also help for better " + "synchronization if framerates and/or delays are different. %s%s", name_.c_str(), + topicQueueSize_, + syncQueueSize_, approxSync_? - uFormat("If topics are not published at the same rate, you could increase \"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); @@ -1096,7 +1104,7 @@ void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequen { if(syncDiagnostic_.get()) { - syncDiagnostic_->tick(stamp, targetFrequency); + syncDiagnostic_->tickOutput(stamp, targetFrequency); } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index b7c67c55..2de994a5 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -35,6 +35,7 @@ void CommonDataSubscriber::depthCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -48,6 +49,7 @@ void CommonDataSubscriber::depthScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -60,6 +62,7 @@ void CommonDataSubscriber::depthScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -72,6 +75,7 @@ void CommonDataSubscriber::depthScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -88,6 +92,7 @@ void CommonDataSubscriber::depthInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -101,6 +106,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -113,6 +119,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -125,6 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null std::vector globalDescriptor; @@ -142,6 +150,7 @@ void CommonDataSubscriber::depthOdomCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -155,6 +164,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -167,6 +177,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -179,6 +190,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; @@ -195,6 +207,7 @@ void CommonDataSubscriber::depthOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -208,6 +221,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -220,6 +234,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -232,6 +247,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -249,6 +265,7 @@ void CommonDataSubscriber::depthDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -262,6 +279,7 @@ void CommonDataSubscriber::depthDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -274,6 +292,7 @@ void CommonDataSubscriber::depthDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -286,6 +305,7 @@ void CommonDataSubscriber::depthDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; @@ -302,6 +322,7 @@ void CommonDataSubscriber::depthDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -315,6 +336,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -327,6 +349,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -339,6 +362,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -356,6 +380,7 @@ void CommonDataSubscriber::depthOdomDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -369,6 +394,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -381,6 +407,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -393,6 +420,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -409,6 +437,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); @@ -422,6 +451,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); } @@ -434,6 +464,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); } @@ -446,6 +477,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) { @@ -457,6 +489,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupDepthCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -466,202 +499,200 @@ void CommonDataSubscriber::setupDepthCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup depth callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); - imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile()); - + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); + #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } #endif @@ -670,57 +701,57 @@ void CommonDataSubscriber::setupDepthCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - 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 4d4b93e2..3389ae70 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -32,6 +32,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::odomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -41,6 +42,7 @@ void CommonDataSubscriber::odomInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); @@ -50,6 +52,7 @@ void CommonDataSubscriber::odomDataCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); } @@ -58,6 +61,7 @@ void CommonDataSubscriber::odomDataInfoCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);} sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); } @@ -65,30 +69,29 @@ void CommonDataSubscriber::odomDataInfoCallback( void CommonDataSubscriber::setupOdomCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup scan callback"); if(subscribeUserData || subscribeOdomInfo) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_); + SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_); } } else @@ -96,13 +99,13 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_); } } else { - odomSubOnly_ = node.create_subscription("odom", rclcpp::QoS(1).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1)); + odomSubOnly_ = node.create_subscription("odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", node.get_name(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index 01d92773..2c880ade 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -34,6 +34,7 @@ void CommonDataSubscriber::rgbCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -47,6 +48,7 @@ void CommonDataSubscriber::rgbScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -59,6 +61,7 @@ void CommonDataSubscriber::rgbScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -71,6 +74,7 @@ void CommonDataSubscriber::rgbScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -87,6 +91,7 @@ void CommonDataSubscriber::rgbInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -100,6 +105,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -112,6 +118,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -124,6 +131,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -141,6 +149,7 @@ void CommonDataSubscriber::rgbOdomCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -154,6 +163,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -166,6 +176,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -178,6 +189,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -194,6 +206,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -207,6 +220,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -219,6 +233,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -231,6 +246,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -248,6 +264,7 @@ void CommonDataSubscriber::rgbDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -261,6 +278,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -273,6 +291,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -285,6 +304,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -301,6 +321,7 @@ void CommonDataSubscriber::rgbDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -314,6 +335,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -326,6 +348,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -338,6 +361,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -355,6 +379,7 @@ void CommonDataSubscriber::rgbOdomDataCallback( const sensor_msgs::msg::Image::ConstSharedPtr imageMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -368,6 +393,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -380,6 +406,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -392,6 +419,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; @@ -408,6 +436,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null @@ -421,6 +450,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -433,6 +463,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null cv_bridge::CvImageConstPtr depthMsg;// Null commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -445,6 +476,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);} cv_bridge::CvImageConstPtr depthMsg;// Null std::vector globalDescriptor; if(!scanMsg->global_descriptor.data.empty()) @@ -457,6 +489,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupRGBCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -466,201 +499,199 @@ void CommonDataSubscriber::setupRGBCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_); } } #endif @@ -669,57 +700,57 @@ void CommonDataSubscriber::setupRGBCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - 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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - 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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - 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 43c26f91..75a4acf6 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 { @@ -37,6 +36,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::rgbdCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -62,6 +62,7 @@ void CommonDataSubscriber::rgbdScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -86,6 +87,7 @@ void CommonDataSubscriber::rgbdScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -110,6 +112,7 @@ void CommonDataSubscriber::rgbdScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -133,6 +136,7 @@ void CommonDataSubscriber::rgbdInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -159,6 +163,7 @@ void CommonDataSubscriber::rgbdOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -184,6 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -208,6 +214,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -232,6 +239,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -259,6 +267,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -285,6 +294,7 @@ void CommonDataSubscriber::rgbdDataCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -310,6 +320,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -334,6 +345,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -358,6 +370,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -385,6 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -411,6 +425,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -436,6 +451,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -460,6 +476,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -484,6 +501,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -511,6 +529,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; rtabmap_conversions::toCvShare(image1Msg, rgb, depth); @@ -532,6 +551,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -541,9 +561,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd callback"); @@ -558,152 +576,152 @@ void CommonDataSubscriber::setupRGBDCallbacks( { rgbdSubs_.resize(1); rgbdSubs_[0] = new message_filters::Subscriber; - rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); + SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0])); } } #endif @@ -712,41 +730,41 @@ void CommonDataSubscriber::setupRGBDCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_); } else { @@ -756,7 +774,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( } else { - rgbdSub_ = node.create_subscription("rgbd_image", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1)); + rgbdSub_ = node.create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1)); 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 21f79374..0427997f 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -29,11 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(2); \ std::vector depthMsgs(2); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ @@ -344,6 +344,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD2Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -353,9 +354,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd2 callback"); @@ -363,152 +362,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks( for(int i=0; i<2; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } #endif @@ -517,45 +516,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - 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 7b62912a..af36a52c 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -28,12 +28,12 @@ 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 { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(3); \ std::vector depthMsgs(3); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ @@ -432,6 +432,7 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD3Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -441,9 +442,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDescriptor, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd3 callback"); @@ -451,152 +450,152 @@ void CommonDataSubscriber::setupRGBD3Callbacks( for(int i=0; i<3; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - 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 @@ -605,46 +604,46 @@ void CommonDataSubscriber::setupRGBD3Callbacks( if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - 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 1977a49b..eeb3c9b5 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -29,11 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(4); \ std::vector depthMsgs(4); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ @@ -401,6 +401,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD4Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -410,9 +411,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd4 callback"); @@ -420,152 +419,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks( for(int i=0; i<4; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - 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(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - 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 @@ -574,45 +573,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - 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 ac588cbd..e60cb8fd 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -29,11 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(5); \ std::vector depthMsgs(5); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ @@ -257,14 +257,13 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback( void CommonDataSubscriber::setupRGBD5Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd5 callback"); @@ -272,53 +271,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks( for(int i=0; i<5; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - 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 @@ -326,45 +325,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - 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 10c32560..ff5a6016 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -29,11 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \ std::vector imageMsgs(6); \ std::vector depthMsgs(6); \ rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ @@ -275,14 +275,13 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback( void CommonDataSubscriber::setupRGBD6Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbd6 callback"); @@ -290,53 +289,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks( for(int i=0; i<6; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - 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 @@ -344,45 +343,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - 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 e2e10a49..9fd8abaa 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -29,11 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { #define IMAGE_CONVERSION() \ + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imagesMsg->header.stamp);} \ UASSERT(!imagesMsg->rgbd_images.empty()); \ std::vector imageMsgs(imagesMsg->rgbd_images.size()); \ std::vector depthMsgs(imagesMsg->rgbd_images.size()); \ @@ -320,6 +320,7 @@ void CommonDataSubscriber::rgbdXOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDXCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -329,157 +330,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback"); - rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); + rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_); } } #endif @@ -488,46 +487,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; RCLCPP_WARN(node.get_logger(), "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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_); } else { rgbdXSub_.unsubscribe(); - rgbdXSubOnly_ = node.create_subscription("rgbd_images", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1)); + rgbdXSubOnly_ = node.create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1)); 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 ac7ebcae..d50b40f6 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -32,6 +32,7 @@ namespace rtabmap_sync { void CommonDataSubscriber::scan2dCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -41,6 +42,7 @@ void CommonDataSubscriber::scan2dCallback( void CommonDataSubscriber::scan3dCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -50,6 +52,7 @@ void CommonDataSubscriber::scan3dCallback( void CommonDataSubscriber::scanDescCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -59,6 +62,7 @@ void CommonDataSubscriber::scan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null @@ -68,6 +72,7 @@ void CommonDataSubscriber::scan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -77,6 +82,7 @@ void CommonDataSubscriber::scanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -86,6 +92,7 @@ void CommonDataSubscriber::odomScan2dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -95,6 +102,7 @@ void CommonDataSubscriber::odomScan3dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -104,6 +112,7 @@ void CommonDataSubscriber::odomScanDescCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -113,6 +122,7 @@ void CommonDataSubscriber::odomScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -122,6 +132,7 @@ void CommonDataSubscriber::odomScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -131,6 +142,7 @@ void CommonDataSubscriber::odomScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -140,6 +152,7 @@ void CommonDataSubscriber::dataScan2dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -149,6 +162,7 @@ void CommonDataSubscriber::dataScan3dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -158,6 +172,7 @@ void CommonDataSubscriber::dataScanDescCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); @@ -167,6 +182,7 @@ void CommonDataSubscriber::dataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -176,6 +192,7 @@ void CommonDataSubscriber::dataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -185,6 +202,7 @@ void CommonDataSubscriber::dataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -194,6 +212,7 @@ void CommonDataSubscriber::odomDataScan2dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); @@ -203,6 +222,7 @@ void CommonDataSubscriber::odomDataScan3dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); @@ -212,6 +232,7 @@ void CommonDataSubscriber::odomDataScanDescCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } @@ -221,6 +242,7 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback( const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::PointCloud2 scan3dMsg; // null commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg); } @@ -230,6 +252,7 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} sensor_msgs::msg::LaserScan scan2dMsg; // Null commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg); } @@ -239,12 +262,14 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback( const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);} commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor); } #endif void CommonDataSubscriber::setupScanCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool scan2dTopic, bool scanDescTopic, bool subscribeOdom, @@ -253,9 +278,7 @@ void CommonDataSubscriber::setupScanCallbacks( #else bool, #endif - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup scan callback"); @@ -268,36 +291,36 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else if(scan2dTopic) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -305,12 +328,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_); } } else @@ -318,12 +341,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_); } } } @@ -331,19 +354,19 @@ void CommonDataSubscriber::setupScanCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_); } } else if(scan2dTopic) @@ -351,12 +374,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_); } } else @@ -364,31 +387,31 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_); } else { - 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(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -396,12 +419,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_); } } else @@ -409,12 +432,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_); } } } @@ -422,18 +445,18 @@ void CommonDataSubscriber::setupScanCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { - SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, 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_); } } } @@ -442,7 +465,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSubOnly_ = node.create_subscription("scan_descriptor", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1)); + scanDescSubOnly_ = node.create_subscription("scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", node.get_name(), @@ -451,7 +474,7 @@ void CommonDataSubscriber::setupScanCallbacks( else if(scan2dTopic) { subscribedToScan2d_ = true; - scan2dSubOnly_ = node.create_subscription("scan", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1)); + scan2dSubOnly_ = node.create_subscription("scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", node.get_name(), @@ -460,7 +483,7 @@ void CommonDataSubscriber::setupScanCallbacks( else { subscribedToScan3d_ = true; - scan3dSubOnly_ = node.create_subscription("scan_cloud", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1)); + scan3dSubOnly_ = node.create_subscription("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", node.get_name(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index e9f3b4f2..3aed5411 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -29,64 +29,65 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include namespace rtabmap_sync { // SensorData void CommonDataSubscriber::sensorDataCallback( - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg) + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::sensorDataInfoCallback( - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg, + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } // SensorData + Odom void CommonDataSubscriber::sensorDataOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg) + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::sensorDataOdomInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, - const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg, + const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { - commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg); + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);} + commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg); } void CommonDataSubscriber::setupSensorDataCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup SensorData callback"); - sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(1).reliability(qosSensorData_).get_rmw_qos_profile()); + sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_); - + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_); + SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_); } } else @@ -94,13 +95,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_); } else { sensorDataSub_.unsubscribe(); - sensorDataSubOnly_ = node.create_subscription("sensor_data", rclcpp::QoS(1).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1)); + sensorDataSubOnly_ = node.create_subscription("sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1)); 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 c09192f1..d9d9ff16 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -36,6 +36,7 @@ void CommonDataSubscriber::stereoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // null @@ -50,6 +51,7 @@ void CommonDataSubscriber::stereoInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null @@ -65,6 +67,7 @@ void CommonDataSubscriber::stereoOdomCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -79,6 +82,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { + if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);} rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scan2dMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -87,32 +91,31 @@ void CommonDataSubscriber::stereoOdomInfoCallback( void CommonDataSubscriber::setupStereoCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); image_transport::TransportHints hints(&node); - imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); - imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); } } else @@ -120,12 +123,12 @@ void CommonDataSubscriber::setupStereoCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile()); - SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - 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 70f5cfb9..f129edc4 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" @@ -47,13 +51,24 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int queueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 10; bool approxSync = true; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCaminfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); @@ -61,8 +76,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize); - RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); + RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); @@ -71,20 +87,20 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_); exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2)); } image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile()); + imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile()); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", get_name(), @@ -99,8 +115,11 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", this->get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"Parameter \"approx_sync\" is false, which means that input " "topics should have all the exact timestamp for the callback to be called.", subscribedTopicsMsg.c_str())); @@ -119,7 +138,7 @@ void RGBSync::callback( const sensor_msgs::msg::Image::ConstSharedPtr image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { - syncDiagnostic_->tick(image->header.stamp); + syncDiagnostic_->tickInput(image->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp); @@ -169,6 +188,7 @@ void RGBSync::callback( stamp, rtabmap_conversions::timestampFromROS(image->header.stamp)); } } + syncDiagnostic_->tickOutput(image->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 076ee4c0..56132aca 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" @@ -46,16 +50,27 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_(1.0), decimation_(1), compressedRate_(0), + approxSyncMaxInterval_(0.0), approxSyncDepth_(0), exactSyncDepth_(0) { - int queueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 10; bool approxSync = true; - double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); - approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); depthScale_ = this->declare_parameter("depth_scale", depthScale_); @@ -69,9 +84,10 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) - RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize); - RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); + RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval_); + RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo); RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_); RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_); @@ -82,26 +98,29 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); - if(approxSyncMaxInterval > 0.0) - approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + if(approxSyncMaxInterval_ > 0.0) + approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } - image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", "raw"); + std::string depthImageTransport = this->declare_parameter("depth_image_transport", "raw"); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageSub_.getSubscriber().getTopic().c_str(), imageDepthSub_.getSubscriber().getTopic().c_str(), cameraInfoSub_.getSubscriber()->get_topic_name()); @@ -112,8 +131,11 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"Parameter \"approx_sync\" is false, which means that input " "topics should have all the exact timestamp for the callback to be called.", subscribedTopicsMsg.c_str())); @@ -130,19 +152,20 @@ void RGBDSync::callback( const sensor_msgs::msg::Image::ConstSharedPtr depth, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { - syncDiagnostic_->tick(image->header.stamp); + syncDiagnostic_->tickInput(image->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp); double depthStamp = rtabmap_conversions::timestampFromROS(depth->header.stamp); double stampDiff = fabs(rgbStamp - depthStamp); - if(stampDiff > 0.010) + if(stampDiff > 0.010 && approxSyncMaxInterval_ == 0.0) { RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " "to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use " - "approx_sync=false if streams have all the exact same timestamp.", + "approx_sync=false if streams have all the exact same timestamp. Setting approx_sync_max_interval " + "will suppress this warning.", stampDiff, rgbStamp, depthStamp); @@ -254,6 +277,7 @@ void RGBDSync::callback( depthStamp, rtabmap_conversions::timestampFromROS(depth->header.stamp)); } } + syncDiagnostic_->tickOutput(image->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index b2fbf8e6..e1ada274 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -42,21 +42,33 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : SYNC_INIT(rgbd7), SYNC_INIT(rgbd8) { - int queueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 10; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize); + RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras); @@ -68,14 +80,14 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : for(int i=0; i; - rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); } std::string name_ = get_name(); 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -83,7 +95,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -91,7 +103,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -99,7 +111,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -107,7 +119,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -115,7 +127,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -123,7 +135,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : } 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); @@ -139,8 +151,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init("", uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"Parameter \"approx_sync\" is false, which means that input " "topics should have all the exact timestamp for the callback to be called.", subscribedTopicsMsg.c_str())); @@ -161,13 +176,14 @@ void RGBDXSync::rgbd2Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(2); output.rgbd_images[0]=(*image0); output.rgbd_images[1]=(*image1); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd3Callback( @@ -175,7 +191,7 @@ void RGBDXSync::rgbd3Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(3); @@ -183,6 +199,7 @@ void RGBDXSync::rgbd3Callback( output.rgbd_images[1]=(*image1); output.rgbd_images[2]=(*image2); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd4Callback( @@ -191,7 +208,7 @@ void RGBDXSync::rgbd4Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(4); @@ -200,6 +217,7 @@ void RGBDXSync::rgbd4Callback( output.rgbd_images[2]=(*image2); output.rgbd_images[3]=(*image3); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd5Callback( @@ -209,7 +227,7 @@ void RGBDXSync::rgbd5Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(5); @@ -219,6 +237,7 @@ void RGBDXSync::rgbd5Callback( output.rgbd_images[3]=(*image3); output.rgbd_images[4]=(*image4); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd6Callback( @@ -229,7 +248,7 @@ void RGBDXSync::rgbd6Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(6); @@ -240,6 +259,7 @@ void RGBDXSync::rgbd6Callback( output.rgbd_images[4]=(*image4); output.rgbd_images[5]=(*image5); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd7Callback( @@ -251,7 +271,7 @@ void RGBDXSync::rgbd7Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(7); @@ -263,6 +283,7 @@ void RGBDXSync::rgbd7Callback( output.rgbd_images[5]=(*image5); output.rgbd_images[6]=(*image6); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } void RGBDXSync::rgbd8Callback( @@ -275,7 +296,7 @@ void RGBDXSync::rgbd8Callback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7) { - syncDiagnostic_->tick(image0->header.stamp); + syncDiagnostic_->tickInput(image0->header.stamp); rtabmap_msgs::msg::RGBDImages output; output.header = image0->header; output.rgbd_images.resize(8); @@ -288,6 +309,7 @@ void RGBDXSync::rgbd8Callback( output.rgbd_images[6]=(*image6); output.rgbd_images[7]=(*image7); rgbdImagesPub_->publish(output); + syncDiagnostic_->tickOutput(image0->header.stamp); } } diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index a5c78f0f..f9408406 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" @@ -46,21 +50,33 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int queueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); - RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize); - RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); + RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); @@ -69,22 +85,22 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::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(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } image_transport::TransportHints hints(this); - imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(); - cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(); + cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", get_name(), @@ -101,8 +117,11 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"Parameter \"approx_sync\" is false, which means that input " "topics should have all the exact timestamp for the callback to be called.", subscribedTopicsMsg.c_str())); @@ -120,7 +139,7 @@ void StereoSync::callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight) { - syncDiagnostic_->tick(imageLeft->header.stamp); + syncDiagnostic_->tickInput(imageLeft->header.stamp); if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count()) { double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp); @@ -190,6 +209,7 @@ void StereoSync::callback( rightStamp, rtabmap_conversions::timestampFromROS(imageRight->header.stamp)); } } + syncDiagnostic_->tickOutput(imageLeft->header.stamp); } } diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index becf84dd..525d239e 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -56,6 +56,10 @@ SET(Libraries rtabmap_conversions ) +if("$ENV{ROS_DISTRO}" STRLESS "jazzy") + add_definitions(-DPRE_ROS_JAZZY) +endif() + ########### ## Build ## ########### @@ -73,6 +77,7 @@ SET(rtabmap_util_plugins_lib_src src/nodelets/lidar_deskewing.cpp src/nodelets/rgbd_relay.cpp src/nodelets/rgbd_split.cpp + src/nodelets/map_assembler.cpp ) @@ -134,6 +139,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudT rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ObstaclesDetection") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAggregator") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAssembler") +rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::MapAssembler") add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp) ament_target_dependencies(rtabmap_rgbd_relay ${Libraries}) @@ -150,10 +156,10 @@ set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") #target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins) #set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer") -#add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) -#ament_target_dependencies(rtabmap_map_assembler ${Libraries}) -#target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins rtabmap_util_plugins) -#set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") +add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) +ament_target_dependencies(rtabmap_map_assembler ${Libraries}) +target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins) +set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp) ament_target_dependencies(rtabmap_imu_to_tf ${Libraries}) @@ -241,7 +247,6 @@ install(TARGETS RUNTIME DESTINATION bin ) install(TARGETS -# rtabmap_map_assembler # rtabmap_map_optimizer # rtabmap_data_player # rtabmap_odom_msg_to_tf @@ -256,6 +261,7 @@ install(TARGETS rtabmap_obstacles_detection rtabmap_point_cloud_aggregator rtabmap_point_cloud_assembler + rtabmap_map_assembler DESTINATION lib/${PROJECT_NAME} ) diff --git a/rtabmap_util/include/rtabmap_util/map_assembler.hpp b/rtabmap_util/include/rtabmap_util/map_assembler.hpp new file mode 100644 index 00000000..8c87adeb --- /dev/null +++ b/rtabmap_util/include/rtabmap_util/map_assembler.hpp @@ -0,0 +1,104 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include "rclcpp/rclcpp.hpp" + +#include +#include "rtabmap_msgs/msg/map_data.hpp" +#include "rtabmap_util/MapsManager.h" + +#include + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +#include +#endif +#endif + +namespace rtabmap_util +{ + +class MapAssembler: public rclcpp::Node +{ + +public: + RTABMAP_UTIL_PUBLIC + explicit MapAssembler(const rclcpp::NodeOptions & options); + virtual ~MapAssembler(); + +private: + void mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg); + + void processMapData(const rtabmap_msgs::msg::MapData & msg); + + void reset(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr); + + void timerCallback(); + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + void octomapBinaryCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res); + + void octomapFullCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res); +#endif +#endif + +private: + MapsManager mapsManager_; + std::map nodes_; + std::map optimizedPoses_; + std::string mapFrameId_; + std::string rtabmapNodeName_; + + rclcpp::Subscription::SharedPtr mapDataSub_; + + rclcpp::Service::SharedPtr resetService_; + + rclcpp::CallbackGroup::SharedPtr serviceCbGroup_; + rclcpp::CallbackGroup::SharedPtr timerCbGroup_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Client::SharedPtr client_; + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + rclcpp::Service::SharedPtr octomapBinarySrv_; + rclcpp::Service::SharedPtr octomapFullSrv_; +#endif +#endif + bool localGridsRegenerated_; +}; + +} \ No newline at end of file diff --git a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp index 6cadfc1a..9610886a 100644 --- a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp +++ b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp @@ -56,6 +56,8 @@ private: rtabmap::LocalGridMaker localMapMaker_; bool mapFrameProjection_; bool warned_; + float rangeMin_; + float rangeMax_; std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; diff --git a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp index 83abad34..dbad53a6 100644 --- a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp +++ b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp @@ -57,8 +57,8 @@ private: const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg); private: - image_transport::CameraPublisher depthImage16Pub_; - image_transport::CameraPublisher depthImage32Pub_; + image_transport::Publisher depthImage16Pub_; + image_transport::Publisher depthImage32Pub_; rclcpp::Publisher::SharedPtr cameraInfo16Pub_; rclcpp::Publisher::SharedPtr cameraInfo32Pub_; rclcpp::Publisher::SharedPtr pointCloudTransformedPub_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 0c386d72..3d9bdafa 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -2,7 +2,7 @@ rtabmap_util - 0.21.5 + 0.22.0 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake + ros_environment + cv_bridge image_transport rclcpp diff --git a/rtabmap_util/scripts/yaml_to_camera_info.py b/rtabmap_util/scripts/yaml_to_camera_info.py index e6a172d8..88bec9f1 100755 --- a/rtabmap_util/scripts/yaml_to_camera_info.py +++ b/rtabmap_util/scripts/yaml_to_camera_info.py @@ -8,6 +8,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) msg = CameraInfo() 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/MapAssemblerNode.cpp b/rtabmap_util/src/MapAssemblerNode.cpp index 3b92c221..db37543a 100644 --- a/rtabmap_util/src/MapAssemblerNode.cpp +++ b/rtabmap_util/src/MapAssemblerNode.cpp @@ -25,349 +25,17 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include "rtabmap_msgs/MapData.h" -#include "rtabmap_conversions/MsgConversion.h" -#include "rtabmap_util/MapsManager.h" -#include "rtabmap_msgs/GetMap.h" -#include -#include -#include -#include -#include -#include -#include +#include "rtabmap_util/map_assembler.hpp" + #include -#include -#include -#include -#include -#include -#include -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP -#include -#include -#include -#endif -#endif - -using namespace rtabmap; - -class MapAssembler +int main(int argc, char **argv) { - -public: - MapAssembler(int & argc, char** argv) : - localGridsRegenerated_(false) - { - ros::NodeHandle pnh("~"); - ros::NodeHandle nh; - - std::string configPath; - pnh.param("config_path", configPath, configPath); - pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_); - - //parameters - rtabmap::ParametersMap parameters; - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); - uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM")); - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp()))); - if(!configPath.empty()) - { - if(UFile::exists(configPath.c_str())) - { - ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str()); - rtabmap::ParametersMap allParameters; - Parameters::readINI(configPath.c_str(), allParameters); - // only update odometry parameters - for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - ParametersMap::iterator jter = allParameters.find(iter->first); - if(jter!=allParameters.end()) - { - iter->second = jter->second; - } - } - } - else - { - ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str()); - } - } - for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - std::string vStr; - bool vBool; - int vInt; - double vDouble; - if(pnh.getParam(iter->first, vStr)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str()); - iter->second = vStr; - } - else if(pnh.getParam(iter->first, vBool)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); - } - else if(pnh.getParam(iter->first, vDouble)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); - } - else if(pnh.getParam(iter->first, vInt)) - { - ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); - } - } - - rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv); - for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter) - { - rtabmap::ParametersMap::iterator jter = parameters.find(iter->first); - if(jter!=parameters.end()) - { - ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str()); - jter->second = iter->second; - } - } - - // Backward compatibility - for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); - iter!=Parameters::getRemovedParameters().end(); - ++iter) - { - std::string vStr; - if(pnh.getParam(iter->first, vStr)) - { - if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) - { - // can be migrated - parameters.at(iter->second.second)= vStr; - ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", - ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); - } - else - { - if(iter->second.second.empty()) - { - ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!", - ros::this_node::getName().c_str(), iter->first.c_str()); - } - else - { - ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", - ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str()); - } - } - } - } - - // set private parameters - for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - pnh.setParam(iter->first, iter->second); - } - - ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false"); - mapsManager_.init(nh, pnh, ros::this_node::getName(), false); - mapsManager_.backwardCompatibilityParameters(pnh, parameters); - mapsManager_.setParameters(parameters); - - std::list splitName = uSplit(nh.resolveName("mapData"), '/'); - std::string rtabmapNs; - for(std::list::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter) - { - if(!rtabmapNs.empty()) - { - rtabmapNs += "/"; - } - rtabmapNs += *iter; - } - ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str()); - if(rtabmapNs.empty()) - { - rtabmapNs = "get_map_data"; - } - else - { - rtabmapNs += "/get_map_data"; - } - - rtabmap_msgs::GetMap getMapSrv; - getMapSrv.request.global = false; - getMapSrv.request.optimized = true; - getMapSrv.request.graphOnly = false; - if(ros::service::waitForService(rtabmapNs, 5000)) - { - if(!ros::service::call(rtabmapNs, getMapSrv)) - { - ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str()); - } - else - { - ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str()); - processMapData(getMapSrv.response.data); - ROS_INFO("Called \"%s\" service, initializing cache... done! The map" - " will be assembled on next subscriber connection.", rtabmapNs.c_str()); - } - } - else - { - ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, " - "may not be a problem if rtabmap is started afterwards. If rtabmap " - "is started after in localization mode, call /rtabmap/publish_maps " - "service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str()); - } - - - mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); - - // private services - resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this); - octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this); -#endif -#endif - } - - ~MapAssembler() - { - } - - void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg) - { - processMapData(*msg); - } - void processMapData(const rtabmap_msgs::MapData & msg) - { - UTimer timer; - - std::map poses; - std::multimap constraints; - Transform mapOdom; - rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom); - for(unsigned int i=0; ifirst) != nodes_.end()) - { - Signature tmpS = nodes_.at(poses.rbegin()->first); - SensorData tmpData = tmpS.sensorData(); - tmpData.setId(0); - uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData))); - poses.insert(std::make_pair(0, poses.rbegin()->second)); - } - - // Update maps - if(!nodes_.empty()) - { - poses = mapsManager_.updateMapCaches( - poses, - 0, - false, - false, - nodes_); - } - double updateTime = timer.ticks(); - - mapFrameId_ = msg.header.frame_id; - optimizedPoses_ = poses; - - mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id); - - ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false"); - } - - bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("map_assembler: reset!"); - mapsManager_.clear(); - return true; - } - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - bool octomapBinaryCallback( - octomap_msgs::GetOctomap::Request &req, - octomap_msgs::GetOctomap::Response &res) - { - ROS_INFO("Sending binary map data on service request"); - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); - - const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); - bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); - return success; - } - - bool octomapFullCallback( - octomap_msgs::GetOctomap::Request &req, - octomap_msgs::GetOctomap::Response &res) - { - ROS_INFO("Sending full map data on service request"); - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); - - const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); - bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); - return success; - } -#endif -#endif - -private: - rtabmap_util::MapsManager mapsManager_; - std::map nodes_; - std::map optimizedPoses_; - std::string mapFrameId_; - - ros::Subscriber mapDataTopic_; - - ros::ServiceServer resetService_; -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - ros::ServiceServer octomapBinarySrv_; - ros::ServiceServer octomapFullSrv_; -#endif -#endif - bool localGridsRegenerated_; -}; - - -int main(int argc, char** argv) -{ - ULogger::setLevel(ULogger::kError); ULogger::setType(ULogger::kTypeConsole); - - ros::init(argc, argv, "map_assembler"); + ULogger::setLevel(ULogger::kError); // process "--params" argument + std::vector arguments; for(int i=1;i(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 414906f7..24a41da4 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1362,7 +1362,11 @@ void MapsManager::publishMaps( (elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_))) { grid_map_msgs::msg::GridMap::UniquePtr msg; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8) + msg = grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap()); +#else msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap()); +#endif msg->header.frame_id = mapFrameId; msg->header.stamp = stamp; elevationMapPub_->publish(std::move(msg)); diff --git a/rtabmap_util/src/PointCloudAssemblerNode.cpp b/rtabmap_util/src/PointCloudAssemblerNode.cpp index 0c16a43a..3c54b469 100644 --- a/rtabmap_util/src/PointCloudAssemblerNode.cpp +++ b/rtabmap_util/src/PointCloudAssemblerNode.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. int main(int argc, char **argv) { + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); rclcpp::init(argc, argv); rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); diff --git a/rtabmap_util/src/PointCloudToDepthImageNode.cpp b/rtabmap_util/src/PointCloudToDepthImageNode.cpp index bb889f2e..0a4f081e 100644 --- a/rtabmap_util/src/PointCloudToDepthImageNode.cpp +++ b/rtabmap_util/src/PointCloudToDepthImageNode.cpp @@ -25,10 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap_util/pointcloud_to_depthimage.hpp" int main(int argc, char **argv) { + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); rclcpp::init(argc, argv); rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index 46f62561..d5b896b6 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 { @@ -38,13 +42,11 @@ namespace rtabmap_util DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : rclcpp::Node("disparity_to_depth", options) { - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); - auto node = rclcpp::Node::make_shared(this->get_name()); - image_transport::ImageTransport it(node); - pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); sub_ = create_subscription("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); } diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 4cb9f056..636ba256 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -27,8 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include +#include namespace rtabmap_util { @@ -42,7 +43,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) : tfListener_ = std::make_shared(*tfBuffer_); tfBroadcaster_ = std::make_shared(this); - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_); qos = this->declare_parameter("qos", qos); @@ -63,8 +64,7 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { tf2::Quaternion q; tf2::fromMsg(msg->orientation, q); - tf2::Transform st; - st.setRotation(q); + tf2::Transform st(q); std::string childFrameId = msg->header.frame_id; @@ -81,10 +81,12 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) return; } - geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp); + geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp); tf2::Transform tmp_t; tf2::fromMsg(tmp.transform, tmp_t); - tf2::Transform t = tmp_t.inverse()*st*tmp_t; + tf2::Quaternion q; + q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation())); + tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation st.setRotation(t.getRotation()); childFrameId = baseFrameId_; } @@ -94,7 +96,6 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) return; } } - st.setOrigin(tf2::Vector3(0,0,0)); geometry_msgs::msg::TransformStamped output; output.header.frame_id = fixedFrameId_; diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index 44eaf570..2ebd00a7 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -14,14 +14,10 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) : slerp_(false) { tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); int queueSize = 5; - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; queueSize = this->declare_parameter("queue_size", queueSize); qos = this->declare_parameter("qos", qos); fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); @@ -77,6 +73,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared sensor_msgs::msg::PointCloud2 scanOutDeskewed; rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); + scanOutDeskewed.header.frame_id = msg->header.frame_id; pubScan_->publish(scanOutDeskewed); } diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp new file mode 100644 index 00000000..f966533c --- /dev/null +++ b/rtabmap_util/src/nodelets/map_assembler.cpp @@ -0,0 +1,361 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include + +#include +#include +#include + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +#include +#include +#endif +#endif + +#ifdef PRE_ROS_JAZZY +namespace rclcpp{ + rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;} +} +#endif + +using namespace std::chrono_literals; + +namespace rtabmap_util +{ +MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : + Node("map_assembler", options), + rtabmapNodeName_("rtabmap"), + localGridsRegenerated_(false) +{ + std::string configPath; + configPath = this->declare_parameter("config_path", configPath); + localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_); + rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); + + //parameters + rtabmap::ParametersMap parameters; + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM")); + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp()))); + if(!configPath.empty()) + { + if(UFile::exists(configPath.c_str())) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Loading parameters from %s", configPath.c_str()); + rtabmap::ParametersMap allParameters; + rtabmap::Parameters::readINI(configPath.c_str(), allParameters); + // only update odometry parameters + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rtabmap::ParametersMap::iterator jter = allParameters.find(iter->first); + if(jter!=allParameters.end()) + { + iter->second = jter->second; + } + } + } + else + { + RCLCPP_ERROR(this->get_logger(), "Config file \"%s\" not found!", configPath.c_str()); + } + } + + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rclcpp::Parameter parameter; + std::string vStr = this->declare_parameter(iter->first, iter->second); + if(vStr.compare(iter->second)!=0) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + } + + std::vector tmpList = this->get_node_options().arguments(); + std::vector argList; + for(unsigned int i=0; i v = uSplit(tmpList[i]); + for(std::list::iterator iter=v.begin(); iter!=v.end(); ++iter) + { + argList.push_back(*iter); + } + } + + char ** argv = new char*[argList.size()]; + for(unsigned int i=0; ifirst); + if(jter!=parameters.end()) + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Update parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + jter->second = iter->second; + } + else + { + RCLCPP_INFO(this->get_logger(), "MapAssembler: Ignored parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + } + } + + // Backward compatibility + for(std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin(); + iter!=rtabmap::Parameters::getRemovedParameters().end(); + ++iter) + { + rclcpp::Parameter parameter; + if(get_parameter(iter->first, parameter)) + { + std::string vStr = parameter.as_string(); + if(!iter->second.second.empty() && parameters.find(iter->second.second)!=parameters.end()) + { + RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".", + iter->first.c_str(), iter->second.second.c_str(), parameters.find(iter->second.second)->second.c_str(), vStr.c_str()); + } + else if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) + { + // can be migrated + parameters.at(iter->second.second)= vStr; + RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); + } + else + { + if(iter->second.second.empty()) + { + RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } + } + } + } + + RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false"); + mapsManager_.init(*this, this->get_name(), true); + mapsManager_.backwardCompatibilityParameters(*this, parameters); + mapsManager_.setParameters(parameters); + + const std::string servicePrefix = get_name() + std::string("/"); + resetService_ = this->create_service(servicePrefix + "reset", std::bind(&MapAssembler::reset, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP + octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&MapAssembler::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&MapAssembler::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); +#endif +#endif + + std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; + + // We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards + serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_ = this->create_client(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer + timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_); +} + +MapAssembler::~MapAssembler() {} + +void MapAssembler::timerCallback() +{ + // Just do this callback one time + timer_->cancel(); + if(mapDataSub_.get()) + { + // double call? ignore + return; + } + + std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; + RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str()); + + if(client_->wait_for_service(5s)) + { + auto request = std::make_shared(); + request->global_map = false; + request->optimized = true; + request->graph_only = false; + + auto future = client_->async_send_request(request); + std::future_status status = future.wait_for(10s); + + if (status == std::future_status::ready) { + RCLCPP_INFO(this->get_logger(), "Initializing cache..."); + processMapData(future.get()->data); + RCLCPP_INFO(this->get_logger(), "Initializing cache... done! The map" + " will be assembled on next subscriber connection."); + } + else + { + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not responding after waiting for 10 seconds.", + getMapSrv.c_str()); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, " + "may not be a problem if rtabmap is started afterwards. If rtabmap " + "is started after in localization mode, call %s/publish_maps " + "service with graph_only=false to make sure map_assembler has all the data.", + getMapSrv.c_str(), + rtabmapNodeName_.c_str()); + } + + rclcpp::SubscriptionOptions options; + options.callback_group = timerCbGroup_; + mapDataSub_ = create_subscription("mapData", rclcpp::QoS(1), + std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options); +} + +void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg) +{ + processMapData(*msg); +} +void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg) +{ + UTimer timer; + + std::map poses; + std::multimap constraints; + rtabmap::Transform mapOdom; + rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom); + for(unsigned int i=0; ifirst) != nodes_.end()) + { + rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first); + rtabmap::SensorData tmpData = tmpS.sensorData(); + tmpData.setId(0); + uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData))); + poses.insert(std::make_pair(0, poses.rbegin()->second)); + } + + // Update maps + if(!nodes_.empty()) + { + poses = mapsManager_.updateMapCaches( + poses, + 0, + false, + false, + nodes_); + } + double updateTime = timer.ticks(); + + mapFrameId_ = msg.header.frame_id; + optimizedPoses_ = poses; + + mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id); + + RCLCPP_INFO(this->get_logger(), "map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false"); +} + +void MapAssembler::reset(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + RCLCPP_INFO(this->get_logger(), "map_assembler: reset!"); + mapsManager_.clear(); +} + +#ifdef WITH_OCTOMAP_MSGS +#ifdef RTABMAP_OCTOMAP +void MapAssembler::octomapBinaryCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Sending binary map data on service request"); + res->map.header.frame_id = mapFrameId_; + res->map.header.stamp = now(); + + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + if(octomap->octree()->size()) + octomap_msgs::binaryMapToMsg(*octomap->octree(), res->map); +} + +void MapAssembler::octomapFullCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Sending full map data on service request"); + res->map.header.frame_id = mapFrameId_; + res->map.header.stamp = now(); + + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + if(octomap->octree()->size()) + octomap_msgs::fullMapToMsg(*octomap->octree(), res->map); +} +#endif +#endif + +} + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::MapAssembler) diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index ac535b7b..0f697caf 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -45,7 +45,9 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : frameId_("base_link"), waitForTransform_(0.2), mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()), - warned_(false) + warned_(false), + rangeMin_(0), + rangeMax_(0) { ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); @@ -53,7 +55,7 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : frameId_ = this->declare_parameter("frame_id", frameId_); mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_); waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); @@ -75,6 +77,8 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : } localMapMaker_.parseParameters(gridParameters); + rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMin(), rangeMin_); + rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMax(), rangeMax_); tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock()); tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_); @@ -86,6 +90,42 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) : cloudSub_ = create_subscription("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1)); } +pcl::PointCloud rangeFiltering( + const pcl::PointCloud & cloud, + float rangeMin, + float rangeMax) +{ + if(!cloud.empty() && (rangeMin > 0.0f || rangeMax > 0.0f)) + { + pcl::PointCloud output; + output.reserve(cloud.size()); + int oi = 0; + float rangeMinSqrd = rangeMin * rangeMin; + float rangeMaxSqrd = rangeMax * rangeMax; + for(size_t i=0; i 0.0f && r < rangeMinSqrd) + { + continue; + } + if(rangeMax > 0.0f && r > rangeMaxSqrd) + { + continue; + } + + output.push_back(pt); + ++oi; + } + output.resize(oi); + return output; + } + + return cloud; +} + void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { rclcpp::Time time = now(); @@ -137,6 +177,11 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar inputCloud->is_dense = true; } + if(rangeMin_ > 0.0f || rangeMax_ > 0.0f) + { + *inputCloud = rangeFiltering(*inputCloud, rangeMin_, rangeMax_); + } + //Common variables for all strategies pcl::IndicesPtr ground, obstacles; pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 33b4a0de..4891aee7 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -59,12 +59,23 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); - int queueSize = 5; + int topicQueueSize = 1; + int syncQueueSize = 5; int count = 2; bool approx=true; double approxSyncMaxInterval = 0.0; - int qos=0; - queueSize = this->declare_parameter("queue_size", queueSize); + int qos=RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); frameId_ = this->declare_parameter("frame_id", frameId_); fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); @@ -76,78 +87,78 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) cloudPub_ = create_publisher("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); - cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); std::string subscribedTopicsMsg; if(count == 4) { - cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); if(approx) { - approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); if(approxSyncMaxInterval > 0.0) approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } else { - exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); exactSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", get_name(), approx?"approx":"exact", approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", - cloudSub_1_.getTopic().c_str(), - cloudSub_2_.getTopic().c_str(), - cloudSub_3_.getTopic().c_str(), - cloudSub_4_.getTopic().c_str()); + cloudSub_1_.getSubscriber()->get_topic_name(), + cloudSub_2_.getSubscriber()->get_topic_name(), + cloudSub_3_.getSubscriber()->get_topic_name(), + cloudSub_4_.getSubscriber()->get_topic_name()); } else if(count == 3) { - cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); if(approx) { - approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); if(approxSyncMaxInterval > 0.0) approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { - exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); exactSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", this->get_name(), approx?"approx":"exact", approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", - cloudSub_1_.getTopic().c_str(), - cloudSub_2_.getTopic().c_str(), - cloudSub_3_.getTopic().c_str()); + cloudSub_1_.getSubscriber()->get_topic_name(), + cloudSub_2_.getSubscriber()->get_topic_name(), + cloudSub_3_.getSubscriber()->get_topic_name()); } else { if(approx) { - approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); if(approxSyncMaxInterval > 0.0) approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2)); } else { - exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); exactSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", this->get_name(), approx?"approx":"exact", approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", - cloudSub_1_.getTopic().c_str(), - cloudSub_2_.getTopic().c_str()); + cloudSub_1_.getSubscriber()->get_topic_name(), + cloudSub_2_.getSubscriber()->get_topic_name()); } @@ -164,10 +175,11 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) this->get_name(), approx?"":"Parameter \"approx_sync\" is false, which means that input " "topics should have all the exact timestamp for the callback to be called.", - subscribedTopicsMsg.c_str()); + subscribedTopicsMsg.c_str()); } } }); + RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str()); } PointCloudAggregator::~PointCloudAggregator() diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index b575c906..99b64ccf 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -73,11 +73,22 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); - int queueSize = 5; - int qos = 0; + int topicQueueSize = 10; + int syncQueueSize = 10; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool subscribeOdomInfo = false; - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosOdom = this->declare_parameter("qos_odom", qos); fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); @@ -97,7 +108,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : removeZ_ = this->declare_parameter("remove_z", removeZ_); subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo); - RCLCPP_INFO(this->get_logger(), "%s: queue_size=%d", get_name(), queueSize); + RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size=%d", get_name(), topicQueueSize); + RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size=%d", get_name(), syncQueueSize); RCLCPP_INFO(this->get_logger(), "%s: qos=%d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_odom=%d", get_name(), qosOdom); RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str()); @@ -118,7 +130,7 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : if(maxClouds_==0 && assemblingTime_ ==0.0) { - RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!"); + RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_clouds or assembling_time parameters should be set!"); exit(-1); } @@ -128,34 +140,34 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : if(!fixedFrameId_.empty()) { - cloudSub_ = create_subscription("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1)); + cloudSub_ = create_subscription("cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s", get_name(), cloudSub_->get_topic_name()); } else if(subscribeOdomInfo) { - syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); - syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); - exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); + syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); + syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); + exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", get_name(), - syncCloudSub_.getTopic().c_str(), - syncOdomSub_.getTopic().c_str(), - syncOdomInfoSub_.getTopic().c_str()); + syncCloudSub_.getSubscriber()->get_topic_name(), + syncOdomSub_.getSubscriber()->get_topic_name(), + syncOdomInfoSub_.getSubscriber()->get_topic_name()); } else { - syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); - exactSync_ = new message_filters::Synchronizer(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_); + syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile()); + exactSync_ = new message_filters::Synchronizer(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_); exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2)); subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", get_name(), - syncCloudSub_.getTopic().c_str(), - syncOdomSub_.getTopic().c_str()); + syncCloudSub_.getSubscriber()->get_topic_name(), + syncOdomSub_.getSubscriber()->get_topic_name()); } warningThread_ = new std::thread([&](){ @@ -274,13 +286,14 @@ void PointCloudAssembler::callbackCloudOdomInfo( } else { - RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received."); + RCLCPP_WARN(this->get_logger(), "Resetting point cloud assembler as null odometry has been received."); clouds_.clear(); } } void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { + callbackCalled_ = true; if(cloudPub_->get_subscription_count()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index f92efbbf..9cd7bde8 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" @@ -62,14 +66,25 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : exactSyncDepth_(0), exactSyncDisparity_(0) { - int queueSize = 10; - int qos = 0; + int topicQueueSize = 1; + int syncQueueSize = 10; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); maxDepth_ = this->declare_parameter("max_depth", maxDepth_); @@ -120,33 +135,33 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_); exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2)); } cloudPub_ = create_publisher("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); image_transport::TransportHints hints(this); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); - disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); } PointCloudXYZ::~PointCloudXYZ() diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index e228e3f5..fb5bd560 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" @@ -67,12 +73,23 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : { bool approxSync = true; std::string roiStr; - int queueSize = 10; - int qos = 0; + int topicQueueSize = 1; + int syncQueueSize = 10; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); maxDepth_ = this->declare_parameter("max_depth", maxDepth_); @@ -135,49 +152,49 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : cloudPub_ = create_publisher("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); - rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1)); + rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1)); 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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::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(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::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(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::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(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); - imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); } PointCloudXYZRGB::~PointCloudXYZRGB() diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index d563d2c1..869b922b 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -60,10 +60,21 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); - int queueSize = 10; - int qos = 0; + int topicQueueSize = 10; + int syncQueueSize = 10; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; bool approx = true; - queueSize = this->declare_parameter("queue_size", queueSize); + topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); + int queueSize = this->declare_parameter("queue_size", -1); + if(queueSize != -1) + { + syncQueueSize = queueSize; + RCLCPP_WARN(this->get_logger(), "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); + } + syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize); qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); @@ -86,7 +97,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), "Params:"); RCLCPP_INFO(this->get_logger(), " approx=%s", approx?"true":"false"); - RCLCPP_INFO(this->get_logger(), " queue_size=%d", queueSize); + RCLCPP_INFO(this->get_logger(), " topic_queue_size=%d", topicQueueSize); + RCLCPP_INFO(this->get_logger(), " sync_queue_size=%d", syncQueueSize); RCLCPP_INFO(this->get_logger(), " fixed_frame_id=%s", fixedFrameId_.c_str()); RCLCPP_INFO(this->get_logger(), " wait_for_transform=%fs", waitForTransform_); RCLCPP_INFO(this->get_logger(), " fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); @@ -95,28 +107,26 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_); RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_); - auto node = rclcpp::Node::make_shared(this->get_name()); - image_transport::ImageTransport it(node); - depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm - depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters + depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm + depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters pointCloudTransformedPub_ = create_publisher("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cameraInfo16Pub_ = create_publisher(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); cameraInfo32Pub_ = create_publisher(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); if(approx) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_); approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::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(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2)); } - pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); + pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile()); } PointCloudToDepthImage::~PointCloudToDepthImage() @@ -149,6 +159,10 @@ void PointCloudToDepthImage::callback( if(cloudDisplacement.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!", + pointCloud2Msg->header.frame_id.c_str(), + cameraInfoMsg->header.frame_id.c_str(), + fixedFrameId_.c_str()); return; } @@ -161,6 +175,9 @@ void PointCloudToDepthImage::callback( if(cloudToCamera.isNull()) { + RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!", + pointCloud2Msg->header.frame_id.c_str(), + cameraInfoMsg->header.frame_id.c_str()); return; } rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera; @@ -229,7 +246,7 @@ void PointCloudToDepthImage::callback( if(depthImage32Pub_.getNumSubscribers()) { depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg); + depthImage32Pub_.publish(depthImage.toImageMsg()); if(cameraInfo32Pub_->get_subscription_count()) { cameraInfo32Pub_->publish(cameraInfoMsgOut); @@ -240,7 +257,7 @@ void PointCloudToDepthImage::callback( { depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image); - depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg); + depthImage16Pub_.publish(depthImage.toImageMsg()); if(cameraInfo16Pub_->get_subscription_count()) { cameraInfo16Pub_->publish(cameraInfoMsgOut); diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index f33add2a..45749ccd 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" @@ -48,7 +52,7 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) : compress_(false), uncompress_(false) { - int qos = 0; + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); compress_ = this->declare_parameter("compress", compress_); uncompress_ = this->declare_parameter("uncompress", uncompress_); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index a7cd3417..d1b85f97 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 { @@ -35,20 +39,15 @@ namespace rtabmap_util RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : Node("rgbd_split", options) { - int queueSize = 10; - int qos = 0; - queueSize = this->declare_parameter("queue_size", queueSize); + int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); - RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - auto node = rclcpp::Node::make_shared(this->get_name()); - image_transport::ImageTransport it(node); - rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); } diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 013895db..a7707f22 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -5,6 +5,25 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + # issues #1285 #1288 + find_library( + builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + rcutils_LIB NAMES rcutils + PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) + find_library( + crypto_LIB NAMES crypto + PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + ) +endif() + find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) @@ -16,6 +35,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 ) @@ -37,8 +58,15 @@ SET(Libraries ## Build ## ########### -add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) +add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp include/${PROJECT_NAME}/PreferencesDialogROS.h) ament_target_dependencies(rtabmap_viz ${Libraries}) +SET_TARGET_PROPERTIES( + rtabmap_viz + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) ############# ## Install ## diff --git a/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h b/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h index 7acee601..6f1a6360 100644 --- a/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h +++ b/rtabmap_viz/include/rtabmap_viz/PreferencesDialogROS.h @@ -36,6 +36,7 @@ using namespace rtabmap; class PreferencesDialogROS : public PreferencesDialog { + Q_OBJECT public: PreferencesDialogROS(rclcpp::Node * node, const QString & configFile, const std::string & rtabmapNodeName); virtual ~PreferencesDialogROS(); @@ -44,7 +45,7 @@ public: virtual QString getTmpIniFilePath() const; bool hasAllParameters(); -public slots: +public Q_SLOTS: void readRtabmapNodeParameters(); protected: diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index a00d3fc9..b8e2ce78 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -2,7 +2,7 @@ rtabmap_viz - 0.21.5 + 0.22.0 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge geometry_msgs rclcpp diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 8220291b..0da4ef78 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -75,10 +75,6 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : maxOdomUpdateRate_(10) { tfBuffer_ = std::make_shared(this->get_clock()); - //auto timer_interface = std::make_shared( - // this->get_node_base_interface(), - // this->get_node_timers_interface()); - //tfBuffer_->setCreateTimerInterface(timer_interface); tfListener_ = std::make_shared(*tfBuffer_); QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; @@ -129,7 +125,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : initCachePath = UDirectory::currentDir(true) + initCachePath; } RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str()); - if(!callMapDataService("get_map_data", false, true, true)) + if(!callMapDataService(rtabmapNodeName_+"/get_map_data", false, true, true)) { RCLCPP_ERROR(this->get_logger(), "The cache will still be loaded " @@ -142,32 +138,32 @@ 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) { RCLCPP_INFO(this->get_logger(), "rtabmap_viz: subscribe_info_only=true"); - infoOnlyTopic_ = this->create_subscription("info", 1, std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1)); + infoOnlyTopic_ = this->create_subscription("info", rclcpp::QoS(this->getTopicQueueSize()), std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1)); } else { - infoTopic_.subscribe(this, "info"); - mapDataTopic_.subscribe(this, "mapData"); + infoTopic_.subscribe(this, "info", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile()); + mapDataTopic_.subscribe(this, "mapData", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile()); infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(this->getQueueSize()), + MyInfoMapSyncPolicy(this->getSyncQueueSize()), infoTopic_, mapDataTopic_); infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2)); } - goalTopic_.subscribe(this, "goal_node"); - pathTopic_.subscribe(this, "global_path"); + goalTopic_.subscribe(this, "goal_node", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile()); + pathTopic_.subscribe(this, "global_path", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile()); goalPathSync_ = new message_filters::Synchronizer( - MyGoalPathSyncPolicy(this->getQueueSize()), + MyGoalPathSyncPolicy(this->getSyncQueueSize()), goalTopic_, pathTopic_); goalPathSync_->registerCallback(std::bind(&GuiWrapper::goalPathCallback, this, std::placeholders::_1, std::placeholders::_2)); - goalReachedTopic_ = this->create_subscription("goal_reached", 5, std::bind(&GuiWrapper::goalReachedCallback, this, std::placeholders::_1)); + goalReachedTopic_ = this->create_subscription("goal_reached", rclcpp::QoS(topicQueueSize_), std::bind(&GuiWrapper::goalReachedCallback, this, std::placeholders::_1)); setupCallbacks(*this); // do it at the end } @@ -335,7 +331,6 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters(); std::vector rosParameters; - auto node = rclcpp::Node::make_shared("rtabmap_viz"); for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) { //save only parameters with valid names @@ -369,7 +364,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 +385,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 +393,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 +413,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 +424,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 +433,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 +463,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 +473,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 +490,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(); @@ -549,8 +544,10 @@ void GuiWrapper::commonMultiCameraCallback( std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -584,9 +581,18 @@ void GuiWrapper::commonMultiCameraCallback( odomHeader = imageMsgs[0]->header; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -727,7 +733,7 @@ void GuiWrapper::commonMultiCameraCallback( cameraModels, 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -750,8 +756,10 @@ void GuiWrapper::commonStereoCallback( { std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -777,9 +785,18 @@ void GuiWrapper::commonStereoCallback( odomHeader = leftCamInfoMsg.header; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -907,7 +924,7 @@ void GuiWrapper::commonStereoCallback( stereoModel, 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -923,8 +940,10 @@ void GuiWrapper::commonLaserScanCallback( { std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -950,9 +969,18 @@ void GuiWrapper::commonLaserScanCallback( return; } odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -1054,7 +1082,7 @@ void GuiWrapper::commonLaserScanCallback( rtabmap::CameraModel(), 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -1069,21 +1097,20 @@ void GuiWrapper::commonOdomCallback( std_msgs::msg::Header odomHeader = odomMsg->header; - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, odomMsg->child_frame_id, odomHeader.stamp, *tfBuffer_, waitForTransform_); + Transform odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) + + UASSERT(odomMsg->twist.covariance.size() == 36); + if(odomMsg->twist.covariance[0] != 0 && + odomMsg->twist.covariance[7] != 0 && + odomMsg->twist.covariance[14] != 0 && + odomMsg->twist.covariance[21] != 0 && + odomMsg->twist.covariance[28] != 0 && + odomMsg->twist.covariance[35] != 0) { - UASSERT(odomMsg->twist.covariance.size() == 36); - if(odomMsg->twist.covariance[0] != 0 && - odomMsg->twist.covariance[7] != 0 && - odomMsg->twist.covariance[14] != 0 && - odomMsg->twist.covariance[21] != 0 && - odomMsg->twist.covariance[28] != 0 && - odomMsg->twist.covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); - } + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); } + if(odomHeader.frame_id.empty()) { RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?"); @@ -1126,7 +1153,7 @@ void GuiWrapper::commonOdomCallback( rtabmap::CameraModel(), 0, rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); @@ -1140,8 +1167,10 @@ void GuiWrapper::commonSensorDataCallback( UASSERT(sensorDataMsg.get()); std_msgs::msg::Header odomHeader; std::string frameId = frameId_; + Transform odomT; if(odomMsg.get()) { + odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); odomHeader = odomMsg->header; if(!odomMsg->child_frame_id.empty()) { @@ -1156,9 +1185,18 @@ void GuiWrapper::commonSensorDataCallback( { odomHeader = sensorDataMsg->header; odomHeader.frame_id = odomFrameId_; + + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); + if(odomT.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " + "TF for stamp %f, aborting! To show red screen in rtabmap_viz " + "when this happens (indicating potentially lost), set subscribe_odom " + "to true.", rclcpp::Time(odomHeader.stamp).seconds()); + return; + } } - Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, *tfBuffer_, waitForTransform_); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { @@ -1229,7 +1267,7 @@ void GuiWrapper::commonSensorDataCallback( info.reg.covariance = covariance; rtabmap::OdometryEvent odomEvent( data, - odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT, + odomT, info); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index d3e64897..cecd841e 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -85,8 +85,7 @@ QString PreferencesDialogROS::getParamMessage() bool PreferencesDialogROS::hasAllParameters() { - auto node = std::make_shared("rtabmap_viz"); - auto client = std::make_shared(node, rtabmapNodeName_); + auto client = std::make_shared(node_, rtabmapNodeName_); return client->service_is_ready(); } @@ -98,8 +97,13 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) path = filePath; } - auto node = std::make_shared("rtabmap_viz"); - RCLCPP_INFO(node->get_logger(), "%s", this->getParamMessage().toStdString().c_str()); + char nodeName[42]; + snprintf( + nodeName, sizeof(nodeName), "rtabmap_viz_param_client_%zx", + reinterpret_cast(this) + ); + auto node = std::make_shared(nodeName); + RCLCPP_INFO(node_->get_logger(), "%s", this->getParamMessage().toStdString().c_str()); rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); // remove Odom parameters for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();) @@ -145,7 +149,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) auto client = std::make_shared(node, rtabmapNodeName_); if (!client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(node->get_logger(), "Can't call rtabmap parameters service, is the node running?"); + RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?"); } int readCount = 0; if(client->service_is_ready()) @@ -163,18 +167,18 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) } } - RCLCPP_INFO(node->get_logger(), "Parameters read = %d", readCount); + RCLCPP_INFO(node_->get_logger(), "Parameters read = %d", readCount); if(readCount>0) { - RCLCPP_INFO(node->get_logger(), "Parameters successfully read."); + RCLCPP_INFO(node_->get_logger(), "Parameters successfully read."); } else { if(this->isVisible()) { QString warning = tr("Failed to get RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); - RCLCPP_WARN(node->get_logger(), "%s", warning.toStdString().c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", warning.toStdString().c_str()); QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); } return false;