diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile index 62978bb7..53447987 100644 --- a/.devcontainer/humble/Dockerfile +++ b/.devcontainer/humble/Dockerfile @@ -8,7 +8,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index a1666586..96db65bc 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/jazzy/Dockerfile b/.devcontainer/jazzy/Dockerfile index 13b36c5a..063aa809 100644 --- a/.devcontainer/jazzy/Dockerfile +++ b/.devcontainer/jazzy/Dockerfile @@ -11,7 +11,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index 5e768e9a..447b63a9 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/kilted/Dockerfile b/.devcontainer/kilted/Dockerfile index 9aee0a83..60af4061 100644 --- a/.devcontainer/kilted/Dockerfile +++ b/.devcontainer/kilted/Dockerfile @@ -11,7 +11,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/.devcontainer/kilted/devcontainer.json b/.devcontainer/kilted/devcontainer.json index 6176ee77..bc93b422 100644 --- a/.devcontainer/kilted/devcontainer.json +++ b/.devcontainer/kilted/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/lyrical/Dockerfile b/.devcontainer/lyrical/Dockerfile new file mode 100644 index 00000000..2cb082f0 --- /dev/null +++ b/.devcontainer/lyrical/Dockerfile @@ -0,0 +1,22 @@ + +FROM introlab3it/rtabmap:resolute + +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ + chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws + +RUN echo "source /opt/ros/lyrical/setup.bash" >> /home/${USERNAME}/.bashrc +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/lyrical/devcontainer.json b/.devcontainer/lyrical/devcontainer.json new file mode 100644 index 00000000..b91675ec --- /dev/null +++ b/.devcontainer/lyrical/devcontainer.json @@ -0,0 +1,39 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "pull": true + }, + "remoteUser": "vscode", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools-themes", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "ms-azuretools.vscode-docker", + "ms-python.python"] + } + }, + "settings": { + "python.autoComplete.extraPaths": [ + "/opt/ros/kilted/lib/python3/dist-packages" + ], + "terminal.integrated.defaultProfile.linux": "bash" + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", + "workspaceFolder": "/home/vscode/ros2_ws", + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=\"rtabmap nav2_msgs grid_map_ros nav2_costmap_2d nav2_bringup realsense2_camera velodyne\" -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } +} diff --git a/.devcontainer/noetic/Dockerfile b/.devcontainer/noetic/Dockerfile index 3eec0a03..d4d636ad 100644 --- a/.devcontainer/noetic/Dockerfile +++ b/.devcontainer/noetic/Dockerfile @@ -8,7 +8,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/catkin_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/catkin_ws diff --git a/.devcontainer/noetic/devcontainer.json b/.devcontainer/noetic/devcontainer.json index 4990bced..43f6becb 100644 --- a/.devcontainer/noetic/devcontainer.json +++ b/.devcontainer/noetic/devcontainer.json @@ -22,6 +22,17 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/catkin_ws", - "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/rolling/Dockerfile b/.devcontainer/rolling/Dockerfile index e763e115..24557f5f 100644 --- a/.devcontainer/rolling/Dockerfile +++ b/.devcontainer/rolling/Dockerfile @@ -1,5 +1,5 @@ -FROM ubuntu:24.04 +FROM ubuntu:26.04 ENV DEBIAN_FRONTEND=noninteractive diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 72a91300..3c280a56 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -2,8 +2,13 @@ name: docker on: push: - branches: - - 'ros2' + branches: [ ros2 ] + pull_request: + branches: [ ros2 ] + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} jobs: docker: @@ -12,7 +17,7 @@ jobs: strategy: fail-fast: false matrix: - docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted-latest] + docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted, kilted-latest, lyrical-latest] include: - docker_tag: humble docker_path: 'humble' @@ -33,8 +38,23 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 + - docker_tag: kilted + docker_path: 'kilted' + docker_platforms: | + linux/amd64 + linux/arm64 - docker_tag: kilted-latest docker_path: 'kilted/latest' + docker_platforms: | + linux/amd64 + #Disabled till rtabmap_ros is released on lyrical + #- docker_tag: lyrical + # docker_path: 'lyrical' + # docker_platforms: | + # linux/amd64 + # linux/arm64 + - docker_tag: lyrical-latest + docker_path: 'lyrical/latest' docker_platforms: | linux/amd64 linux/arm64 @@ -53,6 +73,9 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Login to DockerHub + # Only needed when pushing; skipped on pull requests (secrets are + # unavailable for fork PRs and we don't push there anyway). + if: github.event_name != 'pull_request' uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} @@ -62,8 +85,8 @@ jobs: uses: docker/build-push-action@v6 with: context: . - push: true - platforms: ${{ matrix.docker_platforms }} + push: ${{ github.event_name != 'pull_request' }} + platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }} file: ./docker/${{ matrix.docker_path }}/Dockerfile tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }} no-cache: true diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index aeea113e..28fce40c 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -10,6 +10,10 @@ on: env: BUILD_TYPE: Release +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + jobs: build: name: Build ros2 ${{ matrix.ros_distro }} @@ -19,7 +23,10 @@ jobs: ros_distro: [rolling] include: - ros_distro: rolling - skip_keys: '' + skip_keys: '' # skip keys should be empty when releasing on ROS_DISTRO_devel branch, comment these keys in the appriopriate package. + # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, commented from rtabmap_ros package. + # Make sure rosdistro doesn't declare rtabmap_costmap_plugins. + packages: rtabmap_ros fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full @@ -28,12 +35,16 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} + - run: | + DEBIAN_FRONTEND=noninteractive + sudo apt update + sudo apt upgrade -y - run: | echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: rolling-devel\n" > /tmp/deps.repos &&\ cat /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: rtabmap_ros + package-name: ${{ matrix.packages }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: /tmp/deps.repos rosdep-check: true diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index d24ddaf5..fcbb8551 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -12,6 +12,6 @@ RUN source /ros_entrypoint.sh && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index e2b06208..62259cc4 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -14,6 +14,6 @@ RUN source /ros_entrypoint.sh && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/jazzy/superpoint/Dockerfile b/docker/jazzy/superpoint/Dockerfile new file mode 100644 index 00000000..9fbe0a1e --- /dev/null +++ b/docker/jazzy/superpoint/Dockerfile @@ -0,0 +1,127 @@ +# Latest version with CUDA 12, to be compatible with Opencv 4.12.0 +FROM nvcr.io/nvidia/pytorch:25.06-py3 + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && apt-get install -y \ + libsqlite3-dev \ + git \ + cmake \ + libyaml-cpp-dev \ + software-properties-common \ + pkg-config \ + wget \ + curl \ + build-essential && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Install ros keys +RUN export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" && \ + dpkg -i /tmp/ros2-apt-source.deb + +# Install ros dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y \ + ros-jazzy-ros-base \ + ros-jazzy-rtabmap-ros \ + ros-jazzy-ros-environment \ + ros-jazzy-ament-cmake-auto \ + ros-jazzy-camera-info-manager \ + ros-jazzy-librealsense2 \ + python3-rosdep \ + python3-flake8-docstrings \ + python3-pip \ + python3-pytest-cov \ + ros-dev-tools && \ + apt-get remove -y ros-jazzy-rtabmap* libopencv* && \ + rosdep init && \ + rosdep update && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Optional: MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y && \ + apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Optional: OpenCV with xfeatures2d, cuda and nonfree modules (use same version used by jazzy to avoid cv_bridge conflicts) +RUN git clone -b 4.12.0 https://github.com/opencv/opencv_contrib.git && \ + git clone -b 4.12.0 https://github.com/opencv/opencv.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DOPENCV_EXTRA_MODULES_PATH=/workspace/opencv_contrib/modules \ + -DCMAKE_CXX_STANDARD=17 \ + -DCMAKE_CUDA_STANDARD=17 \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=ON \ + -DBUILD_TESTS=OFF \ + -DBUILD_PERF_TESTS=OFF \ + -DOPENCV_ENABLE_NONFREE=ON \ + -DWITH_VTK=OFF \ + -DWITH_TBB=ON \ + -DWITH_CUDA=ON .. && \ + make -j6 && \ + make install && \ + cd /workspace && \ + rm -rf opencv opencv_contrib + +# Optional: OpenGV (multi-camera support) +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j6 && \ + make install && \ + cd /workspace && \ + rm -r opengv + +# Setup catkin workspace +RUN mkdir -p ros2_ws/src +COPY . ros2_ws/src/rtabmap_ros + +# Get rtabmap library +# Create Superpoint model with current pytorch version +# Setup Superglue +# build ros packages (rebuild all packages depending on opencv) +RUN source /opt/ros/jazzy/setup.bash && \ + git clone -b jazzy https://github.com/ros-perception/image_pipeline.git ros2_ws/src/image_pipeline && \ + git clone -b 4.1.0 https://github.com/ros-perception/vision_opencv.git ros2_ws/src/vision_opencv && \ + git clone -b jazzy https://github.com/ros-perception/image_transport_plugins.git ros2_ws/src/image_transport_plugins && \ + git clone -b r/4.56.4 https://github.com/IntelRealSense/realsense-ros.git ros2_ws/src/realsense-ros && \ + git clone https://github.com/introlab/rtabmap ros2_ws/src/rtabmap && \ + cd ros2_ws/src/rtabmap/archive/2022-IlluminationInvariant/scripts && \ + wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth && \ + wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py && \ + python3 trace.py && \ + mv superpoint_v1.pt /workspace/. && \ + cd /workspace && \ + git clone https://github.com/magicleap/SuperGluePretrainedNetwork && \ + cp ros2_ws/src/rtabmap/corelib/src/python/rtabmap_superglue.py SuperGluePretrainedNetwork/. && \ + cd ros2_ws && \ + export MAKEFLAGS="-j6" && \ + colcon build --install-base /usr/local/ros --event-handlers console_direct+ --cmake-args \ + --no-warn-unused-cli \ + -DTorch_DIR=/usr/local/lib/python3.12/dist-packages/torch/share/cmake/Torch \ + -DWITH_TORCH=ON \ + -DWITH_PYTHON=ON \ + -DRTABMAP_SYNC_MULTI_RGBD=ON \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTING=OFF && \ + cd /workspace && \ + rm -rf ros2_ws + +# Setup ROS entrypoint +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/jazzy/setup.bash"\nsource "/usr/local/ros/setup.bash"\nexec "$@"' > /ros_entrypoint.sh && \ + chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +RUN source /ros_entrypoint.sh && ldconfig \ No newline at end of file diff --git a/docker/jazzy/superpoint/README.md b/docker/jazzy/superpoint/README.md new file mode 100644 index 00000000..09311cb0 --- /dev/null +++ b/docker/jazzy/superpoint/README.md @@ -0,0 +1,59 @@ +Docker image example to include pytorch/CUDA support (SuperPoint, SuperGlue, OpenCV+nonfree+xfeatures2d) + +# Create image: +```bash +cd rtabmap_ros +docker build -t rtabmap_ros:superpoint -f docker/jazzy/superpoint/Dockerfile . +``` +# Example of usage: + +We launch the [realsense_d435i_infra.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/realsense_d435i_infra.launch.py) example with arguments to use superpoint + superglue for loop closure detection. Note that visual odometry is done with default parameters in this case. + +```bash +# X11 Setup for rtabmap_viz, not required if you don't launch any UI +XAUTH=/tmp/.docker.xauth +touch $XAUTH +xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + +# Docker Run Command +docker run -it --rm \ + --user $(id -u) \ + --privileged \ + --gpus all \ + -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e XAUTHORITY=$XAUTH \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -e ROS_HOME=/tmp/.ros \ + --network host \ + -v ~/.ros:/tmp/.ros \ + rtabmap_ros:superpoint \ + ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \ + args:=" \ + --SuperPoint/ModelPath /workspace/superpoint_v1.pt \ + --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \ + --Kp/DetectorStrategy 11 \ + --Kp/NndrRatio 0.6 \ + --Vis/CorNNType 6 \ + --Vis/CorNNDR 0.6 \ + --Reg/RepeatOnce false \ + --Vis/CorGuessWinSize 0" \ + odom_args:=" \ + --Vis/CorNNType 1 \ + --Reg/RepeatOnce true \ + --Vis/CorGuessWinSize 40 \ + --Vis/CorNNDR 0.8" +``` + +The resulting database will be saved to `~/.ros/rtabmap.db` on the host computer. You can also use the `launch.sh` file in this folder for convenience. + +To use superpoint for odometry, remove `odom_args` and add this to `args`: +```bash +--Vis/FeatureType 11 \ +``` + +Performance tip: to avoid extracting again in `rtabmap` superpoint features already extracted in `rgbd_odometry`, we would need to edit `realsense_d435i_infra.launch.py` and add the parameter `subscribe_sensor_data:=true` to `rtabmap` and `rtabmap_viz`, then remap `sensor_data:=odom_sensor_data/raw`. \ No newline at end of file diff --git a/docker/jazzy/superpoint/launch.sh b/docker/jazzy/superpoint/launch.sh new file mode 100755 index 00000000..56b6df97 --- /dev/null +++ b/docker/jazzy/superpoint/launch.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +# X11 Setup +XAUTH=/tmp/.docker.xauth +touch $XAUTH +xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + +# Docker Run Command +docker run -it --rm \ + --user $(id -u) \ + --privileged \ + --gpus all \ + -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e XAUTHORITY=$XAUTH \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -e ROS_HOME=/tmp/.ros \ + --network host \ + -v ~/.ros:/tmp/.ros \ + rtabmap_ros:superpoint \ + ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \ + args:=" \ + --SuperPoint/ModelPath /workspace/superpoint_v1.pt \ + --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \ + --Kp/DetectorStrategy 11 \ + --Kp/NndrRatio 0.6 \ + --Vis/CorNNType 6 \ + --Vis/CorNNDR 0.6 \ + --Reg/RepeatOnce false \ + --Vis/CorGuessWinSize 0" \ + odom_args:=" \ + --Vis/CorNNType 1 \ + --Reg/RepeatOnce true \ + --Vis/CorGuessWinSize 40 \ + --Vis/CorNNDR 0.8" diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index 97cc6c46..685cbec1 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -14,6 +14,6 @@ RUN source /ros_entrypoint.sh && \ apt-get update && \ rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/lyrical/Dockerfile b/docker/lyrical/Dockerfile new file mode 100644 index 00000000..f4e916df --- /dev/null +++ b/docker/lyrical/Dockerfile @@ -0,0 +1,6 @@ +FROM osrf/ros:lyrical-desktop +# install rtabmap packages +RUN apt-get update && apt-get install -y \ + ros-lyrical-rtabmap \ + ros-lyrical-rtabmap-ros \ + && rm -rf /var/lib/apt/lists/ diff --git a/docker/lyrical/latest/Dockerfile b/docker/lyrical/latest/Dockerfile new file mode 100644 index 00000000..6517694d --- /dev/null +++ b/docker/lyrical/latest/Dockerfile @@ -0,0 +1,19 @@ +FROM introlab3it/rtabmap:resolute + +RUN source /ros_entrypoint.sh && \ + mkdir -p ros2_ws/src && \ + cd ros2_ws/src + +COPY . ros2_ws/src/rtabmap_ros + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ + rosdep init && \ + rosdep update && \ + apt-get update && \ + rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros nav2_costmap_2d" && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + colcon build --packages-skip rtabmap_costmap_plugins rtabmap_ros --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + cd && \ + rm -rf ros2_ws diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 37a5b0a9..4dc41907 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -28,15 +28,30 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) -find_package(RTABMap 0.22.0 REQUIRED) - -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/include -) +find_package(RTABMap 0.23.5 REQUIRED) # libraries SET(Libraries + image_geometry::image_geometry + laser_geometry::laser_geometry + pcl_conversions::pcl_conversions + std_msgs::std_msgs + tf2::tf2 + tf2_eigen::tf2_eigen + tf2_geometry_msgs::tf2_geometry_msgs +) +SET(PublicLibraries + sensor_msgs::sensor_msgs + geometry_msgs::geometry_msgs + rtabmap_msgs::rtabmap_msgs + rclcpp::rclcpp + rtabmap::core + cv_bridge::cv_bridge + tf2_ros::tf2_ros +) +SET(AmentLibraries cv_bridge geometry_msgs image_geometry @@ -74,7 +89,11 @@ target_include_directories(rtabmap_conversions $ $ ) -ament_target_dependencies(rtabmap_conversions ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_conversions ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_conversions PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) +ENDIF() IF("$ENV{ROS_DISTRO}" STRLESS "iron") target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON -DPRE_ROS_KILTED) @@ -85,8 +104,7 @@ ENDIF() ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_conversions) # To include downstream without targets diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 05ea8a6c..4e82e388 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MSGCONVERSION_H_ #include "rclcpp/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include #include #include diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index a0c94b20..9ad6cadd 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -2,7 +2,7 @@ rtabmap_conversions - 0.22.1 + 0.23.7 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 @@ -28,6 +28,7 @@ tf2 tf2_eigen tf2_geometry_msgs + tf2_ros ament_cmake diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 3c97a2c4..39dabb6e 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -191,39 +191,45 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_br void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth) { - if(!image.rgb.data.empty()) + try { - rgb = cv_bridge::toCvShare(image.rgb, trackedObject); - } - else if(!image.rgb_compressed.data.empty()) - { - rgb = cv_bridge::toCvCopy(image.rgb_compressed); - } - else - { - // empty - rgb = std::make_shared(); - } - - if(!image.depth.data.empty()) - { - depth = cv_bridge::toCvShare(image.depth, trackedObject); - } - else if(!image.depth_compressed.data.empty()) - { - if(image.depth_compressed.format.compare("jpg")==0) + if(!image.rgb.data.empty()) { - depth = cv_bridge::toCvCopy(image.depth_compressed); + rgb = cv_bridge::toCvShare(image.rgb, trackedObject); + } + else if(!image.rgb_compressed.data.empty()) + { + rgb = cv_bridge::toCvCopy(image.rgb_compressed); } else { - cv_bridge::CvImagePtr ptr = std::make_shared(); - ptr->header = image.depth_compressed.header; - ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); - UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); - ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; - depth = ptr; + // empty + rgb = std::make_shared(); } + + if(!image.depth.data.empty()) + { + depth = cv_bridge::toCvShare(image.depth, trackedObject); + } + else if(!image.depth_compressed.data.empty()) + { + if(image.depth_compressed.format.compare("jpg")==0) + { + depth = cv_bridge::toCvCopy(image.depth_compressed); + } + else + { + cv_bridge::CvImagePtr ptr = std::make_shared(); + ptr->header = image.depth_compressed.header; + ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); + UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); + ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; + depth = ptr; + } + } + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } @@ -348,27 +354,32 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh } cv::Mat left, right; - if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = imageRectLeft->image; + try { + if( imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = imageRectLeft->image; + } + else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image; + } + if( imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + right = imageRectRight->image; + } + else + { + right = cv_bridge::cvtColor(imageRectRight, "mono8")->image; + } } - else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image; - } - if(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - right = imageRectRight->image; - } - else - { - right = cv_bridge::cvtColor(imageRectRight, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } // @@ -420,19 +431,24 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh } cv_bridge::CvImageConstPtr ptrImage = imageMsg; - if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || - imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) - { - // do nothing + try { + if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || + imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + // do nothing + } + else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::cvtColor(imageMsg, "mono8"); + } + else + { + ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8"); + } } - else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::cvtColor(imageMsg, "mono8"); - } - else - { - ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } cv_bridge::CvImageConstPtr ptrDepth = depthMsg; @@ -835,25 +851,6 @@ rtabmap::CameraModel cameraModelFromROS( D.at(0,4) = camInfo.d[2]; D.at(0,5) = camInfo.d[3]; } - else if(camInfo.d.size()>8) - { - bool zerosAfter8 = true; - for(size_t i=8; i(model.D().cols); + memcpy(camInfo.d.data(), model.D().data, model.D().cols*sizeof(double)); + camInfo.distortion_model = "plumb_bob"; + } + else if(model.D_raw().total() == 6) { camInfo.d = std::vector(4); camInfo.d[0] = model.D_raw().at(0,0); @@ -921,7 +928,7 @@ void cameraModelToROS( } UASSERT(model.R().empty() || model.R().total() == 9); - if(model.R().empty()) + if(model.R().empty() || countNonZero(model.R()) == 0) { cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1); memcpy(camInfo.r.data(), eye.data, 9*sizeof(double)); @@ -931,7 +938,6 @@ void cameraModelToROS( memcpy(camInfo.r.data(), model.R().data, 9*sizeof(double)); } - UASSERT(model.P().empty() || model.P().total() == 12); if(model.P().empty()) { memset(camInfo.p.data(), 0.0, 12*sizeof(double)); @@ -969,14 +975,14 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::msg::CameraInfo & leftCamInfo, const sensor_msgs::msg::CameraInfo & rightCamInfo, const std::string & frameId, - tf2_ros::Buffer & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { rtabmap::Transform localTransform = getTransform( frameId, leftCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(localTransform.isNull()) { @@ -987,7 +993,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( leftCamInfo.header.frame_id, rightCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -1146,18 +1152,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) } else { - if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = leftRawPtr->image.clone(); + try { + if( leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = leftRawPtr->image.clone(); + } + else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image; + } } - else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } } @@ -1179,18 +1190,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) } else { - if(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - (!isStereo && - (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0|| - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))) - { - right = rightRawPtr->image.clone(); + try{ + if( rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + (!isStereo && + (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0|| + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))) + { + right = rightRawPtr->image.clone(); + } + else + { + right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image; + } } - else - { - right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } } @@ -1927,7 +1943,7 @@ rtabmap::Landmarks landmarksFromROS( const std::string & frameId, const std::string & odomFrameId, const rclcpp::Time & odomStamp, - tf2_ros::Buffer & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, double defaultLinVariance, double defaultAngVariance) @@ -1945,7 +1961,7 @@ rtabmap::Landmarks landmarksFromROS( frameId, iter->second.first.header.frame_id, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(baseToCamera.isNull()) @@ -1965,7 +1981,7 @@ rtabmap::Landmarks landmarksFromROS( odomFrameId, odomStamp, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(!correction.isNull()) { @@ -1994,7 +2010,7 @@ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, const rclcpp::Time & stamp, - tf2_ros::Buffer &tfBuffer, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { // TF ready? @@ -2007,7 +2023,7 @@ rtabmap::Transform getTransform( } catch(tf2::TransformException & ex) { - UWARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); + UWARN("(getting transform \"%s\" -> \"%s\") %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); } return transform; @@ -2050,7 +2066,7 @@ bool convertRGBDMsgs( cv::Mat & depth, std::vector & cameraModels, std::vector & stereoCameraModels, - tf2_ros::Buffer & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadRectifiedImages, const std::vector > & localKeyPointsMsgs, @@ -2176,7 +2192,7 @@ bool convertRGBDMsgs( } // use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb) - rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform); + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds()); @@ -2190,7 +2206,7 @@ bool convertRGBDMsgs( odomFrameId, odomStamp, stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2207,19 +2223,24 @@ bool convertRGBDMsgs( if(!imageMsgs.empty()) { cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i]; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) - { - // do nothing + try { + if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + // do nothing + } + else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); + } + else + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); + } } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } // initialize @@ -2270,7 +2291,12 @@ bool convertRGBDMsgs( } else { - ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8"); + try{ + ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8"); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } } // initialize @@ -2329,7 +2355,7 @@ bool convertRGBDMsgs( depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2374,7 +2400,7 @@ bool convertRGBDMsgs( cameraInfoMsgs[i].header.frame_id, depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); } if(stereoTransform.isNull() || stereoTransform.x()<=0) @@ -2442,7 +2468,7 @@ bool convertStereoMsg( cv::Mat & left, cv::Mat & right, rtabmap::StereoCameraModel & stereoModel, - tf2_ros::Buffer & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadyRectified) { @@ -2470,30 +2496,35 @@ bool convertStereoMsg( return false; } - if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = leftImageMsg->image.clone(); + try{ + if( leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = leftImageMsg->image.clone(); + } + else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image; + } + if( rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + right = rightImageMsg->image.clone(); + } + else + { + right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; + } } - else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image; - } - if(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - right = rightImageMsg->image.clone(); - } - else - { - right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform); + rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { return false; @@ -2506,7 +2537,7 @@ bool convertStereoMsg( odomFrameId, odomStamp, leftImageMsg->header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2526,7 +2557,7 @@ bool convertStereoMsg( rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2556,7 +2587,7 @@ bool convertStereoMsg( leftCamInfoMsg.header.frame_id, rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull() || stereoTransform.x()<=0) { @@ -2753,7 +2784,7 @@ bool convertScan3dMsg( const std::string & odomFrameId, const rclcpp::Time & odomStamp, rtabmap::LaserScan & scan, - tf2_ros::Buffer & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, int maxPoints, float maxRange, @@ -2762,7 +2793,7 @@ bool convertScan3dMsg( UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height, uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str()); - rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform); + rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfBuffer, waitForTransform); if(scanLocalTransform.isNull()) { UERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", timestampFromROS(scan3dMsg.header.stamp)); @@ -2777,7 +2808,7 @@ bool convertScan3dMsg( odomFrameId, odomStamp, scan3dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt new file mode 100644 index 00000000..d592a057 --- /dev/null +++ b/rtabmap_costmap_plugins/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.5) +project(rtabmap_costmap_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(visualization_msgs REQUIRED) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +SET(Libraries + pluginlib::pluginlib + rclcpp::rclcpp + nav2_costmap_2d::layers + visualization_msgs::visualization_msgs +) +SET(AmentLibraries + pluginlib + rclcpp + nav2_costmap_2d + visualization_msgs +) + +########### +## Build ## +########### + +add_library(rtabmap_costmap_plugins SHARED + src/voxel_layer.cpp +) +target_include_directories(rtabmap_costmap_plugins + PUBLIC + $ + $ +) + +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") + target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY) + ENDIF() + ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries}) +ENDIF() + + + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) + +add_executable(rtabmap_costmap_voxel_marker src/voxel_marker.cpp) +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_costmap_voxel_marker ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_costmap_voxel_marker PRIVATE ${Libraries}) +ENDIF() +set_target_properties(rtabmap_costmap_voxel_marker PROPERTIES OUTPUT_NAME "voxel_marker") + +############# +## Install ## +############# +ament_export_dependencies(${AmentLibraries}) +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}) # To include downstream with targets +ament_export_libraries(rtabmap_costmap_plugins) # To include downstream without targets + +install(TARGETS + rtabmap_costmap_plugins + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(TARGETS + rtabmap_costmap_voxel_marker + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +ament_package() diff --git a/rtabmap_costmap_plugins/COLCON_IGNORE b/rtabmap_costmap_plugins/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/rtabmap_costmap_plugins/costmap_plugins.xml b/rtabmap_costmap_plugins/costmap_plugins.xml new file mode 100644 index 00000000..a0b3e75a --- /dev/null +++ b/rtabmap_costmap_plugins/costmap_plugins.xml @@ -0,0 +1,5 @@ + + + Similar to nav2_costmap_2d::VoxelLayer, but can also move along z-axis. + + \ No newline at end of file diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h new file mode 100644 index 00000000..074d0882 --- /dev/null +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h @@ -0,0 +1,58 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ +#define RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((dllexport)) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT __attribute__ ((dllimport)) + #else + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __declspec(dllexport) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT __declspec(dllimport) + #endif + #ifdef RTABMAP_COSTMAP_PLUGINS_BUILDING_DLL + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_EXPORT + #else + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_IMPORT + #endif + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE RTABMAP_COSTMAP_PLUGINS_PUBLIC + #define RTABMAP_COSTMAP_PLUGINS_LOCAL +#else + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((visibility("default"))) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT + #if __GNUC__ >= 4 + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC __attribute__ ((visibility("default"))) + #define RTABMAP_COSTMAP_PLUGINS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC + #define RTABMAP_COSTMAP_PLUGINS_LOCAL + #endif + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp new file mode 100644 index 00000000..affc632d --- /dev/null +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp @@ -0,0 +1,293 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ +#define RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rtabmap_costmap_plugins +{ + +/** + * @class VoxelLayer + * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment + */ +class VoxelLayer : public nav2_costmap_2d::ObstacleLayer +{ +public: + RTABMAP_COSTMAP_PLUGINS_PUBLIC + VoxelLayer() + : voxel_grid_(0, 0, 0) + { + costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D + } + + /** + * @brief Voxel Layer destructor + */ + virtual ~VoxelLayer(); + + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + + /** + * @brief Update the layer's origin to a new pose, often when in a rolling costmap + */ + void updateOrigin(double new_origin_x, double new_origin_y); + + /** + * @brief If layer is discretely populated + */ + bool isDiscretized() + { + return true; + } + + /** + * @brief Match the size of the master costmap + */ + virtual void matchSize(); + + /** + * @brief Reset this costmap + */ + virtual void reset(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable() {return true;} + +protected: + /** + * @brief Reset internal maps + */ + virtual void resetMaps(); + + /** + * @brief Use raycasting between 2 points to clear freespace + */ + virtual void raytraceFreespace( + const nav2_costmap_2d::Observation & clearing_observation, + double * min_x, double * min_y, + double * max_x, + double * max_y); + + bool publish_voxel_; + std::string robot_base_frame_; + rclcpp::Publisher::SharedPtr voxel_pub_; + nav2_voxel_grid::VoxelGrid voxel_grid_; + double z_resolution_, origin_z_; + int unknown_threshold_, mark_threshold_, size_z_; + rclcpp::Publisher::SharedPtr + clearing_endpoints_pub_; + + /** + * @brief Convert world coordinates into map coordinates + */ + inline bool worldToMap3DFloat( + double wx, double wy, double wz, double & mx, double & my, + double & mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) { + return false; + } + mx = ((wx - origin_x_) / resolution_); + my = ((wy - origin_y_) / resolution_); + mz = ((wz - origin_z_) / z_resolution_); + if (mx < size_x_ && my < size_y_ && mz < size_z_) { + return true; + } + + return false; + } + + /** + * @brief Convert world coordinates into map coordinates + */ + inline bool worldToMap3D( + double wx, double wy, double wz, unsigned int & mx, unsigned int & my, + unsigned int & mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) { + return false; + } + + mx = static_cast((wx - origin_x_) / resolution_); + my = static_cast((wy - origin_y_) / resolution_); + mz = static_cast((wz - origin_z_) / z_resolution_); + + if (mx < size_x_ && my < size_y_ && mz < (unsigned int)size_z_) { + return true; + } + + return false; + } + + /** + * @brief Convert map coordinates into world coordinates + */ + inline void mapToWorld3D( + unsigned int mx, unsigned int my, unsigned int mz, double & wx, + double & wy, + double & wz) + { + // returns the center point of the cell + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; + wz = origin_z_ + (mz + 0.5) * z_resolution_; + } + + /** + * @brief Find L2 norm distance in 3D + */ + inline double dist(double x0, double y0, double z0, double x1, double y1, double z1) + { + return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0)); + } + + /** + * @brief Get the height of the voxel sizes in meters + */ + double getSizeInMetersZ() const + { + return (size_z_ - 1 + 0.5) * z_resolution_; + } + + /** + * @brief Copy a region of a source map into a destination map + * @param source_map The source map + * @param sm_lower_left_x The lower left x point of the source map to start the copy + * @param sm_lower_left_y The lower left y point of the source map to start the copy + * @param sm_size_x The x size of the source map + * @param dest_map The destination map + * @param dm_lower_left_x The lower left x point of the destination map to start the copy + * @param dm_lower_left_y The lower left y point of the destination map to start the copy + * @param dm_size_x The x size of the destination map + * @param region_size_x The x size of the region to copy + * @param region_size_y The y size of the region to copy + */ + template + void copyMapRegion3D( + data_type * source_map, unsigned int sm_lower_left_x, + unsigned int sm_lower_left_y, + unsigned int sm_size_x, data_type * dest_map, unsigned int dm_lower_left_x, + unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, + unsigned int region_size_y, int z_shift) + { + // we'll first need to compute the starting points for each map + // this is like getting voxel column. We are not taking into account the z position of the voxel + data_type * sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x); + data_type * dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x); + + uint32_t marked_bits_mask = (data_type) 0xFFFF0000; + uint32_t unknown_bits_mask = (data_type) 0x0000FFFF; + + // now, we'll copy the source map into the destination map + for (unsigned int i = 0; i < region_size_y; ++i) { + memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)); + + for (unsigned int j = 0; j < region_size_x; j++) { + // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits + if (z_shift > 0) { + dm_index[j] = + // Shift marked cells, insert zeros for new unknowns + ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) | + // Shift empty/unknown cells, insert ones for new unknowns + (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << (sizeof(data_type) * 4 - z_shift))) & unknown_bits_mask); + + } else if (z_shift < 0) { + dm_index[j] = + // Shift marked cells, insert zeros for new unknowns + (dm_index[j] & marked_bits_mask) << z_shift * -1 | + // Shift empty/unknown cells, insert ones for new unknowns + ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1)); + } + } + + sm_index += sm_size_x; + dm_index += dm_size_x; + } + } + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +}; + +} // namespace rtabmap_costmap_plugins + +#endif // RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml new file mode 100644 index 00000000..58ed5be1 --- /dev/null +++ b/rtabmap_costmap_plugins/package.xml @@ -0,0 +1,26 @@ + + + + rtabmap_costmap_plugins + 0.23.7 + RTAB-Map's costmap plugins. + Mathieu Labbe + Mathieu Labbe + BSD + https://github.com/introlab/rtabmap_ros/issues + https://github.com/introlab/rtabmap_ros + + ament_cmake_ros + + ros_environment + + pluginlib + rclcpp + nav2_costmap_2d + visualization_msgs + + + ament_cmake + + + diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp new file mode 100644 index 00000000..dcb6ca3f --- /dev/null +++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp @@ -0,0 +1,600 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "rtabmap_costmap_plugins/voxel_layer.hpp" + +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#define VOXEL_BITS 16 +PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::VoxelLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; + +namespace rtabmap_costmap_plugins +{ + +void VoxelLayer::onInitialize() +{ + nav2_costmap_2d::ObstacleLayer::onInitialize(); + + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); + declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); + declareParameter("z_voxels", rclcpp::ParameterValue(10)); + declareParameter("origin_z", rclcpp::ParameterValue(0.0)); + declareParameter("z_resolution", rclcpp::ParameterValue(0.2)); + declareParameter("unknown_threshold", rclcpp::ParameterValue(15)); + declareParameter("mark_threshold", rclcpp::ParameterValue(0)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); + declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link")); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "z_voxels", size_z_); + node->get_parameter(name_ + "." + "origin_z", origin_z_); + node->get_parameter(name_ + "." + "z_resolution", z_resolution_); + node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); + node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_); + + int combination_method_param{}; + node->get_parameter(name_ + "." + "combination_method", combination_method_param); +#ifdef PRE_ROS_JAZZY + combination_method_ = combination_method_param; +#else + combination_method_ = combination_method_from_int(combination_method_param); +#endif + + if (publish_voxel_) { + voxel_pub_ = node->create_publisher( + "voxel_grid", rclcpp::QoS(1).transient_local()); + //voxel_pub_->on_activate(); + } + + clearing_endpoints_pub_ = node->create_publisher( + "clearing_endpoints", rclcpp::QoS(1).transient_local()); + //clearing_endpoints_pub_->on_activate(); + + unknown_threshold_ += (VOXEL_BITS - size_z_); + matchSize(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &VoxelLayer::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +VoxelLayer::~VoxelLayer() +{ + auto node = node_.lock(); + if (dyn_params_handler_ && node) { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } + dyn_params_handler_.reset(); +} + +void VoxelLayer::matchSize() +{ + std::lock_guard guard(*getMutex()); + ObstacleLayer::matchSize(); + voxel_grid_.resize(size_x_, size_y_, size_z_); + assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); +} + +void VoxelLayer::reset() +{ + // Call the base class method before adding our own functionality + ObstacleLayer::reset(); + resetMaps(); +} + +void VoxelLayer::resetMaps() +{ + // Call the base class method before adding our own functionality + // Note: at the time this was written, ObstacleLayer doesn't implement + // resetMaps so this goes to the next layer down Costmap2DLayer which also + // doesn't implement this, so it actually goes all the way to Costmap2D + ObstacleLayer::resetMaps(); + voxel_grid_.reset(); +} + +void VoxelLayer::updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, double * max_x, double * max_y) +{ + std::lock_guard guard(*getMutex()); + + if (rolling_window_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!enabled_) { + return; + } + useExtraBounds(min_x, min_y, max_x, max_y); + + bool current = true; + std::vector observations, clearing_observations; + + // get the marking observations + current = getMarkingObservations(observations) && current; + + // get the clearing observations + current = getClearingObservations(clearing_observations) && current; + + // update the global current status + current_ = current; + + // raytrace freespace + for (unsigned int i = 0; i < clearing_observations.size(); ++i) { + raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + } + + // place the new obstacles into a priority queue... each with a priority of zero to begin with + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); + ++it) + { + const nav2_costmap_2d::Observation & obs = *it; + + const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); + + double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; + double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; + + sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + // if the obstacle is too low, we won't add it + if (*iter_z < min_obstacle_height_) { + continue; + } + + // if the obstacle is too high or too far away from the robot we won't add it + if (*iter_z > max_obstacle_height_) { + continue; + } + + // compute the squared distance from the hitpoint to the pointcloud's origin + double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x) + + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) + + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z); + + // if the point is far enough away... we won't consider it + if (sq_dist >= sq_obstacle_max_range) { + continue; + } + + // If the point is too close, do not consider it + if (sq_dist < sq_obstacle_min_range) { + continue; + } + + // now we need to compute the map coordinates for the observation + unsigned int mx, my, mz; + if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) { + continue; + } + + // mark the cell in the voxel grid and check if we should also mark it in the costmap + if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) { + unsigned int index = getIndex(mx, my); + + costmap_[index] = LETHAL_OBSTACLE; + touch( + static_cast(*iter_x), static_cast(*iter_y), + min_x, min_y, max_x, max_y); + } + } + } + + if (publish_voxel_) { + auto grid_msg = std::make_unique(); + unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); + grid_msg->size_x = voxel_grid_.sizeX(); + grid_msg->size_y = voxel_grid_.sizeY(); + grid_msg->size_z = voxel_grid_.sizeZ(); + grid_msg->data.resize(size); + memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + + grid_msg->origin.x = origin_x_; + grid_msg->origin.y = origin_y_; + grid_msg->origin.z = origin_z_; + + grid_msg->resolutions.x = resolution_; + grid_msg->resolutions.y = resolution_; + grid_msg->resolutions.z = z_resolution_; + grid_msg->header.frame_id = global_frame_; + grid_msg->header.stamp = clock_->now(); + + voxel_pub_->publish(std::move(grid_msg)); + } + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void VoxelLayer::raytraceFreespace( + const nav2_costmap_2d::Observation & clearing_observation, double * min_x, + double * min_y, + double * max_x, + double * max_y) +{ + auto clearing_endpoints_ = std::make_unique(); + + if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) { + return; + } + + double sensor_x, sensor_y, sensor_z; + double ox = clearing_observation.origin_.x; + double oy = clearing_observation.origin_.y; + double oz = clearing_observation.origin_.z; + + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { + RCLCPP_WARN( + logger_, + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds " + "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, oz, + origin_x_, origin_y_, origin_z_, + origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(), + origin_z_ + getSizeInMetersZ()); + + return; + } + + bool publish_clearing_points; + + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0); + } + + clearing_endpoints_->data.clear(); + clearing_endpoints_->width = clearing_observation.cloud_->width; + clearing_endpoints_->height = clearing_observation.cloud_->height; + clearing_endpoints_->is_dense = true; + clearing_endpoints_->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_); + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_x(*clearing_endpoints_, "x"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_y(*clearing_endpoints_, "y"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_z(*clearing_endpoints_, "z"); + + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later + double map_end_x = origin_x_ + getSizeInMetersX(); + double map_end_y = origin_y_ + getSizeInMetersY(); + double map_end_z = origin_z_ + getSizeInMetersZ(); + + sensor_msgs::PointCloud2ConstIterator iter_x(*(clearing_observation.cloud_), "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + double wpx = *iter_x; + double wpy = *iter_y; + double wpz = *iter_z; + + double distance = dist(ox, oy, oz, wpx, wpy, wpz); + double scaling_fact = 1.0; + scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0); + wpx = scaling_fact * (wpx - ox) + ox; + wpy = scaling_fact * (wpy - oy) + oy; + wpz = scaling_fact * (wpz - oz) + oz; + + double a = wpx - ox; + double b = wpy - oy; + double c = wpz - oz; + double t = 1.0; + bool wp_outside = false; + + // we can only raytrace to a maximum z height + if (wpz > map_end_z) { + // we know we want the vector's z value to be max_z + t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c)); + wp_outside = true; + } else if (wpz < origin_z_) { + // and we can only raytrace down to the floor + // we know we want the vector's z value to be 0.0 + t = std::min(t, (origin_z_ - oz) / c); + wp_outside = true; + } + + // the minimum value to raytrace from is the origin + if (wpx < origin_x_) { + t = std::min(t, (origin_x_ - ox) / a); + wp_outside = true; + } + if (wpy < origin_y_) { + t = std::min(t, (origin_y_ - oy) / b); + wp_outside = true; + } + + // the maximum value to raytrace to is the end of the map + if (wpx > map_end_x) { + t = std::min(t, (map_end_x - ox) / a); + wp_outside = true; + } + if (wpy > map_end_y) { + t = std::min(t, (map_end_y - oy) / b); + wp_outside = true; + } + + constexpr double wp_epsilon = 1e-5; + if (wp_outside) { + if (t > 0.0) { + t -= wp_epsilon; + } else if (t < 0.0) { + t += wp_epsilon; + } + } + + wpx = ox + a * t; + wpy = oy + b * t; + wpz = oz + c * t; + + double point_x, point_y, point_z; + if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) { + unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_); + unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); + + + // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); + voxel_grid_.clearVoxelLineInMap( + sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, + costmap_, + unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, + cell_raytrace_max_range, cell_raytrace_min_range); + + updateRaytraceBounds( + ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_, + clearing_observation.raytrace_min_range_, min_x, min_y, + max_x, + max_y); + + if (publish_clearing_points) { + *clearing_endpoints_iter_x = wpx; + *clearing_endpoints_iter_y = wpy; + *clearing_endpoints_iter_z = wpz; + + ++clearing_endpoints_iter_x; + ++clearing_endpoints_iter_y; + ++clearing_endpoints_iter_z; + } + } + } + + if (publish_clearing_points) { + clearing_endpoints_->header.frame_id = global_frame_; + clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp; + + clearing_endpoints_pub_->publish(std::move(clearing_endpoints_)); + } +} + +void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) +{ + int cell_oz; + // get the global pose of the robot + try + { + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, rclcpp::Time(0)); + + const double robot_z = transformStamped.transform.translation.z; + const double z_grid_height = z_resolution_ * size_z_; + const double new_origin_z = robot_z - z_grid_height / 2; + cell_oz = int((new_origin_z - origin_z_) / z_resolution_); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(logger_, "%s", ex.what()); + // If the robot pose is not detected, the origin_z_ will remain the same. + cell_oz = 0; + } + + // project the new origin into the grid + int cell_ox, cell_oy; + cell_ox = static_cast((new_origin_x - origin_x_) / resolution_); + cell_oy = static_cast((new_origin_y - origin_y_) / resolution_); + + // compute the associated world coordinates for the origin cell + // because we want to keep things grid-aligned + double new_grid_ox, new_grid_oy, new_grid_oz; + new_grid_ox = origin_x_ + cell_ox * resolution_; + new_grid_oy = origin_y_ + cell_oy * resolution_; + new_grid_oz = origin_z_ + cell_oz * z_resolution_; + + // To save casting from unsigned int to int a bunch of times + int size_x = size_x_; + int size_y = size_y_; + + // we need to compute the overlap of the new and existing windows + int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + lower_left_x = std::min(std::max(cell_ox, 0), size_x); + lower_left_y = std::min(std::max(cell_oy, 0), size_y); + upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x); + upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y); + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; + unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; + unsigned int * voxel_map = voxel_grid_.getData(); + + // copy the local window in the costmap to the local map + copyMapRegion( + costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, + cell_size_x, + cell_size_y); + copyMapRegion( + voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, + cell_size_x, + cell_size_y); + + // we'll reset our maps to unknown space if appropriate + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + origin_z_ = new_grid_oz; + + // compute the starting cell location for copying data back in + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion( + local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, + cell_size_y); + copyMapRegion3D( + local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, + cell_size_x, + cell_size_y, + cell_oz); + + // make sure to clean up + delete[] local_map; + delete[] local_voxel_map; +} + +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +VoxelLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + bool resize_map_needed = false; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "min_obstacle_height") { + min_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "max_obstacle_height") { + max_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "origin_z") { + origin_z_ = parameter.as_double(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "z_resolution") { + z_resolution_ = parameter.as_double(); + resize_map_needed = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + current_ = false; + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "publish_voxel_map") { + RCLCPP_WARN( + logger_, "publish voxel map is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update."); + continue; + } + + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "z_voxels") { + size_z_ = parameter.as_int(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "unknown_threshold") { + unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_); + } else if (param_name == name_ + "." + "mark_threshold") { + mark_threshold_ = parameter.as_int(); + } else if (param_name == name_ + "." + "combination_method") { +#ifdef PRE_ROS_JAZZY + combination_method_ = parameter.as_int(); +#else + combination_method_ = combination_method_from_int(parameter.as_int()); +#endif + } + } + } + + if (resize_map_needed) { + matchSize(); + } + + result.successful = true; + return result; +} + +} // namespace rtabmap_costmap_plugins diff --git a/rtabmap_costmap_plugins/src/voxel_marker.cpp b/rtabmap_costmap_plugins/src/voxel_marker.cpp new file mode 100644 index 00000000..5f114901 --- /dev/null +++ b/rtabmap_costmap_plugins/src/voxel_marker.cpp @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +/** + * Modified matlabbe: + * Added option to choose between unknown, free and marked cells + */ + +#include +#include +#include +#include + +namespace rtabmap_costmap_plugins +{ + +// FREE, UNKNOWN, MARKED +double g_voxel_colors_r[] = {0.0, 1.0, 1.0}; +double g_voxel_colors_g[] = {1.0, 1.0, 0.0}; +double g_voxel_colors_b[] = {1.0, 1.0, 0.0}; +double g_voxel_colors_a[] = {0.5, 0.1, 0.5}; + +class VoxelMarker: public rclcpp::Node +{ +public: + explicit VoxelMarker(const rclcpp::NodeOptions & options) : + rclcpp::Node("voxel_marker", options) + { + cell_type_ = this->declare_parameter("cell_type", (int)nav2_voxel_grid::VoxelStatus::MARKED); + color_r_ = this->declare_parameter("r", g_voxel_colors_r[cell_type_]); + color_g_ = this->declare_parameter("g", g_voxel_colors_g[cell_type_]); + color_b_ = this->declare_parameter("b", g_voxel_colors_b[cell_type_]); + color_a_ = this->declare_parameter("a", g_voxel_colors_a[cell_type_]); + + voxel_sub_ = this->create_subscription("voxel_grid", rclcpp::QoS(1), std::bind(&VoxelMarker::voxelCallback, this, std::placeholders::_1)); + marker_pub_ = this->create_publisher("visualization_marker", rclcpp::QoS(1)); + + } + virtual ~VoxelMarker() {} + + void voxelCallback(const nav2_msgs::msg::VoxelGrid::SharedPtr grid) + { + if (grid->data.empty()) + { + RCLCPP_ERROR(get_logger(), "Received empty voxel grid"); + return; + } + + visualization_msgs::msg::Marker m; + m.header.frame_id = grid->header.frame_id; + m.header.stamp = grid->header.stamp; + m.ns = "voxel_grid"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::CUBE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.orientation.w = 1.0; + m.color.r = color_r_; + m.color.g = color_g_; + m.color.b = color_b_; + m.color.a = color_a_; + + const uint32_t* data = &grid->data.front(); + const double x_origin = grid->origin.x; + const double y_origin = grid->origin.y; + const double z_origin = grid->origin.z; + const double x_res = grid->resolutions.x; + const double y_res = grid->resolutions.y; + const double z_res = grid->resolutions.z; + const uint32_t x_size = grid->size_x; + const uint32_t y_size = grid->size_y; + const uint32_t z_size = grid->size_z; + for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) + { + for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) + { + for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) + { + nav2_voxel_grid::VoxelStatus status = nav2_voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, + data); + + if (status == (nav2_voxel_grid::VoxelStatus)cell_type_) + { + geometry_msgs::msg::Point p; + p.x = x_origin + (x_grid + 0.5) * x_res; + p.y = y_origin + (y_grid + 0.5) * y_res; + p.z = z_origin + (z_grid + 0.5) * z_res; + m.points.push_back(p); + } + } + } + } + m.scale.x = x_res; + m.scale.y = y_res; + m.scale.z = z_res; + + marker_pub_->publish(m); + } + +private: + int cell_type_; + double color_r_; + double color_g_; + double color_b_; + double color_a_; + + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr voxel_sub_; +}; + +} // rtabmap_costmap_plugins + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + rclcpp::shutdown(); +} diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index 4f37769c..dfedb2a8 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -8,6 +8,7 @@ + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) + [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)](#turtlebot3-nav2-2d-lidar-slam-with-fusioncore-imu--wheel-ukf) + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) @@ -59,6 +60,12 @@ * Yellow: The map. ![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1) +### Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF) +[turtlebot3_sim_fusioncore_icp_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py) (Jazzy + Gazebo Harmonic) + +FusionCore (wheel + IMU UKF) and `icp_odometry` run in a feedback loop: FusionCore's stable `odom` frame seeds scan matching via `guess_frame_id`, and the ICP result feeds back into FusionCore as a second velocity source. See [README](launch/turtlebot3/fusioncore/README.md) for architecture details. + +![FusionCore icp_odometry demo](https://github.com/user-attachments/assets/e1e07cfb-74e0-48b9-9bfd-32b68ee5a6ef) ### Champ Quadruped Nav2, Elevation Map and VSLAM [champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py) diff --git a/rtabmap_demos/launch/husky/husky_slam2d.launch.py b/rtabmap_demos/launch/husky/husky_slam2d.launch.py index 53bfcf52..667e31ed 100644 --- a/rtabmap_demos/launch/husky/husky_slam2d.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam2d.launch.py @@ -123,6 +123,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings), ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d.launch.py b/rtabmap_demos/launch/husky/husky_slam3d.launch.py index bbadcb25..513f39e0 100644 --- a/rtabmap_demos/launch/husky/husky_slam3d.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam3d.launch.py @@ -141,6 +141,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings), ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py index 62b4116f..0c412ebf 100644 --- a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py @@ -129,6 +129,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]), ]) diff --git a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py index 6ad55bd8..244180da 100644 --- a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py +++ b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py @@ -103,7 +103,8 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(rtabmap_viz), package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace='rtabmap', - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": vo_node_prefix+'_odometry'}], remappings=remappings), ] diff --git a/rtabmap_demos/launch/multisession_mapping_demo.launch.py b/rtabmap_demos/launch/multisession_mapping_demo.launch.py index b5841039..77211c39 100644 --- a/rtabmap_demos/launch/multisession_mapping_demo.launch.py +++ b/rtabmap_demos/launch/multisession_mapping_demo.launch.py @@ -60,6 +60,7 @@ def generate_launch_description(): 'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries 'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries 'Kp/MaxFeatures': '400', + 'Kp/BadSignRatio': '0.25', # Kp/BadSignRatio behaves differently than before if Kp/MaxFeatures is not 0, that is now a ratio of Kp/MaxFeatures directly. 'Reg/Force3DoF': 'true', 'RGBD/OptimizeMaxError': '10', 'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2) diff --git a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py index 41036389..f3b63581 100644 --- a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py +++ b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py @@ -141,7 +141,8 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', condition=IfCondition(LaunchConfiguration("rtabmap_viz")), - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": 'stereo_odometry'}], remappings=remappings), Node( package='rviz2', executable='rviz2', name="rviz2", output='screen', diff --git a/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py new file mode 100644 index 00000000..4510d659 --- /dev/null +++ b/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py @@ -0,0 +1,201 @@ +# Requirements: +# Download one or both rosbags: +# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link +# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link +# +# This is the "composition" variant of stereo_outdoor_demo.launch.py: the whole +# pipeline (image_proc rectification, stereo synchronization, visual odometry +# and SLAM) runs as composable nodes in a single component container +# (rtabmap_container). We can set 'use_intra_process_comms' on all of them. +# That way images are passed between rectify -> disparity/sync -> odometry -> +# SLAM by pointer, without inter-process serialization/copies. +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos stereo_outdoor_demo_composition.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play stereo_outdoorA.db3 --clock +# when done, you can play the secon bag: +# $ ros2 bag play stereo_outdoorB.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node, SetParameter, ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'subscribe_rgbd':True, + 'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs + 'map_negative_poses_ignored':True, + 'subscribe_odom_info': True, + # RTAB-Map's internal parameters should be strings + 'OdomF2M/MaxSize': '1000', + 'GFTT/MinDistance': '10', + 'GFTT/QualityLevel': '0.00001', + #'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + #'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + } + + remappings=[ + ('rgbd_image', '/stereo_camera/rgbd_image'), + ('odom', '/vo')] + + # Enable zero-copy intra-process communication between all composable nodes + # loaded in the container. + intra_process = [{'use_intra_process_comms': True}] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + # ---- image_proc rectification per camera ---- + def image_proc_nodes(side, color=False): + ns = 'stereo_camera/' + side + rectify = ComposableNode( + package='image_proc', plugin='image_proc::RectifyNode', + name='rectify_color_node' if color else 'rectify_mono_node', namespace=ns, + remappings=[ + ('image', 'image_color' if color else 'image_mono'), + ('camera_info', 'camera_info_throttle'), + ('image_rect', 'image_rect_color' if color else 'image_rect')], + extra_arguments=intra_process) + return [ + ComposableNode( + package='image_proc', plugin='image_proc::DebayerNode', + name='debayer_node', namespace=ns, + extra_arguments=intra_process), + rectify, + ] + + # ---- rtabmap pipeline (always-on nodes) ---- + rtabmap_nodes = [ + # Synchronize stereo data together in a single topic + # Issue: stereo_img_proc doesn't produce color and + # grayscale images exactly the same (there is a small + # vertical shift with color), we should use grayscale for + # left and right images to get similar results than on ros1 noetic. + ComposableNode( + package='rtabmap_sync', plugin='rtabmap_sync::StereoSync', + namespace='stereo_camera', + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('right/image_rect', 'right/image_rect'), + ('left/camera_info', 'left/camera_info_throttle'), + ('right/camera_info', 'right/camera_info_throttle')], + extra_arguments=intra_process), + + # Visual odometry + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + ] + + # Name of the shared component container. + container_name = '/rtabmap_container' + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc. + Node( + package='image_transport', executable='republish', name='republish_left', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'left/image_raw_throttle/compressed'), + ('out', 'left/image_raw')]), + Node( + package='image_transport', executable='republish', name='republish_right', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'right/image_raw_throttle/compressed'), + ('out', 'right/image_raw')]), + + # Single component container holding the whole pipeline. All nodes set + # use_intra_process_comms=True, so images are passed by pointer. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions= + image_proc_nodes('left') + + image_proc_nodes('right') + + rtabmap_nodes), + + # SLAM mode (loaded into the shared container): + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible with + # intra-process comms ("intraprocess communication allowed only with + # volatile durability"). Setting latch=False makes those topics volatile + # so the node can join the zero-copy container. Trade-off: viewers that + # start after a map is published won't get the retained last message, + # but rtabmap republishes the map as it updates. + LoadComposableNodes( + condition=UnlessCondition(localization), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d': delete the previous database (~/.ros/rtabmap.db) + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Localization mode (loaded into the shared container): + LoadComposableNodes( + condition=IfCondition(localization), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True', + 'latch': False}], # volatile QoS, see SLAM-mode note above + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component + # in the container above. It is a Qt application and its UI must run in + # the process main thread, while components are loaded in container + # worker threads. So it cannot be composed and does not benefit from + # intra-process comms here (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters, + {"odometry_node_name": 'stereo_odometry'}], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/README.md b/rtabmap_demos/launch/turtlebot3/fusioncore/README.md new file mode 100644 index 00000000..ec5f0a57 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/README.md @@ -0,0 +1,91 @@ +# FusionCore + icp_odometry: TurtleBot3 Gazebo Demo + +This demo shows a feedback loop between [FusionCore](https://github.com/manankharwar/fusioncore) and rtabmap's `icp_odometry` where each node tightens the other. + +## Architecture + +``` +/imu ──────────────────────┐ +/odom (wheel) ──────────────┤──→ FusionCore (UKF) +/rtabmap/icp_odometry ──────┘ │ + ↑ │ publishes: odom → base_footprint TF + │ │ /fusion/odom + │ guess_frame_id: odom│ + └────── icp_odometry ←───────┘ + │ (publish_tf: false) + │ + └──→ /rtabmap/icp_odometry ──→ rtabmap SLAM ──→ map → odom TF +``` + +**What each node contributes:** + +| Node | Input | Provides | +|---|---|---| +| FusionCore | wheels + IMU | stable `odom` frame, continuous state at 100 Hz | +| icp_odometry | `/scan` + FusionCore's `odom` as initial guess | scan-level pose corrections | +| FusionCore encoder2 | icp_odometry output | tighter velocity corrections from ICP | +| rtabmap SLAM | icp_odometry output | global map, loop closures | + +FusionCore gives `icp_odometry` a stable initial guess via `guess_frame_id: odom`. +Better initial guesses mean scan matching succeeds more often and with lower error. +The ICP result feeds back into FusionCore as a second velocity source (`encoder2`), +tightening the state estimate further. `Odom/ResetCountdown: 1` lets the system +auto-recover if ICP loses tracking. + +## Simulation vs real hardware + +In Gazebo, the DiffDrive plugin produces near-perfect wheel velocities with no slip or +encoder noise, while the simulated MPU9250 injects Gaussian noise. FusionCore fusing +both means the noisy IMU slightly degrades what is already a perfect odometry source, +so the `map → odom` correction on each scan update will be slightly larger than in the +standard wheel-odometry-only demo. On real hardware this completely inverts: wheel +encoders accumulate slip, terrain variation, and mechanical error that dwarfs IMU noise, +and fusion pays off measurably. The sim-tuned IMU noise values in `fusioncore_tb3.yaml` +(`gyro_noise: 0.002`, `accel_noise: 0.02`) reduce unnecessary filter uncertainty in +simulation; real MPU9250 users should use the hardware spec values noted in that file. + +## Quick start + +```bash +export TURTLEBOT3_MODEL=waffle + +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py +``` + +Optional arguments: + +```bash +# Localization mode (requires saved map from a previous mapping run) +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true + +# Different Gazebo world +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house +``` + +## Prerequisites + +```bash +sudo apt install ros-jazzy-fusioncore-ros ros-jazzy-turtlebot3-gazebo ros-jazzy-rtabmap-ros ros-jazzy-nav2-bringup +export TURTLEBOT3_MODEL=waffle +``` + +## Files + +| File | Purpose | +|---|---| +| `turtlebot3_sim_fusioncore_icp_demo.launch.py` | Complete demo: Gazebo + FusionCore + rtabmap + Nav2 | +| `turtlebot3_fusioncore_icp.launch.py` | Core only: FusionCore + icp_odometry + rtabmap (no Gazebo) | +| `../../params/fusioncore_tb3.yaml` | FusionCore config for TB3 Waffle | +| `../../params/turtlebot3_fusioncore_icp_nav2_params.yaml` | Nav2 config using `/fusion/odom` | + +## Topic and TF summary + +| Topic / TF | Publisher | Subscribers | +|---|---|---| +| `/imu` | Gazebo | FusionCore | +| `/odom` | Gazebo (wheel) | FusionCore | +| `/scan` | Gazebo (lidar) | icp_odometry, rtabmap | +| `/rtabmap/icp_odometry` | icp_odometry | FusionCore (encoder2), rtabmap | +| `/fusion/odom` | FusionCore | Nav2 | +| TF `odom → base_footprint` | FusionCore | icp_odometry (guess), Nav2 | +| TF `map → odom` | rtabmap | Nav2 | diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py new file mode 100644 index 00000000..ae70a95c --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py @@ -0,0 +1,165 @@ +""" +FusionCore + icp_odometry feedback loop for TurtleBot3. + +Architecture (Option A from rtabmap_ros issue #1418): + + FusionCore (wheels + IMU) + |-- publishes: odom -> base_footprint TF, /fusion/odom + |-- provides initial pose guess to icp_odometry via guess_frame_id + + icp_odometry (/scan) + |-- guess_frame_id: odom (uses FusionCore's stable odom as scan match seed) + |-- publish_tf: false (FusionCore owns the odom TF) + |-- publishes: /rtabmap/icp_odometry + + FusionCore encoder2 + |-- topic: /rtabmap/icp_odometry + |-- ICP corrections fed back as a second velocity source + + rtabmap SLAM + |-- subscribes to /rtabmap/icp_odometry for mapping + |-- Odom/ResetCountdown: 1 for auto-recovery if ICP loses tracking + |-- publishes: map -> odom TF +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, EmitEvent, + OpaqueFunction, RegisterEventHandler, TimerAction) +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LifecycleNode, Node +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +from lifecycle_msgs.msg import Transition + + +def launch_setup(context, *args, **kwargs): + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization').perform(context) + localization = localization in ('True', 'true') + + pkg_demos = get_package_share_directory('rtabmap_demos') + fusioncore_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3.yaml') + + # ── FusionCore lifecycle node ───────────────────────────────────────────── + fc = LifecycleNode( + package='fusioncore_ros', + executable='fusioncore_node', + name='fusioncore', + namespace='', + output='screen', + parameters=[fusioncore_config, {'use_sim_time': use_sim_time}], + remappings=[ + ('/imu/data', '/imu'), # TB3 Gazebo IMU topic + ('/odom/wheels', '/odom'), # TB3 Gazebo wheel odometry topic + ], + ) + + # Wait 2 s for node to spin up, then configure + configure = TimerAction( + period=2.0, + actions=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=lambda a: a is fc, + transition_id=Transition.TRANSITION_CONFIGURE, + ))], + ) + + # As soon as configuring -> inactive, activate + activate = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=fc, + start_state='configuring', + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=lambda a: a is fc, + transition_id=Transition.TRANSITION_ACTIVATE, + ))], + )) + + # ── icp_odometry ────────────────────────────────────────────────────────── + icp_parameters = { + 'frame_id': 'base_footprint', + 'odom_frame_id': 'odom', + 'guess_frame_id': 'odom', + 'publish_tf': False, + 'publish_null_when_lost': False, + 'use_sim_time': use_sim_time, + 'Reg/Strategy': '1', + 'Reg/Force3DoF': 'true', + 'Odom/ResetCountdown': '1', + 'RGBD/NeighborLinkRefining': 'True', + 'Grid/RangeMin': '0.2', + } + + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + output='screen', + parameters=[icp_parameters], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + ) + + # ── rtabmap SLAM ────────────────────────────────────────────────────────── + slam_parameters = { + 'frame_id': 'base_footprint', + 'odom_frame_id': 'odom', + 'use_sim_time': use_sim_time, + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_scan': True, + 'approx_sync': True, + 'use_action_for_goal': True, + 'Reg/Strategy': '1', + 'Reg/Force3DoF': 'true', + 'RGBD/NeighborLinkRefining': 'True', + 'Grid/RangeMin': '0.2', + 'Optimizer/GravitySigma': '0', + } + + if localization: + slam_parameters['Mem/IncrementalMemory'] = 'False' + slam_parameters['Mem/InitWMWithAllNodes'] = 'True' + + rtabmap_args = [] if localization else ['-d'] + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + output='screen', + parameters=[slam_parameters], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + arguments=rtabmap_args, + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + output='screen', + parameters=[slam_parameters, {'odometry_node_name': 'icp_odometry'}], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + ) + + return [fc, configure, activate, icp_odometry_node, rtabmap_node, rtabmap_viz_node] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode (requires existing map)'), + + OpaqueFunction(function=launch_setup), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py new file mode 100644 index 00000000..064531b5 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py @@ -0,0 +1,181 @@ +""" +Complete TurtleBot3 demo: Gazebo (new) + FusionCore + icp_odometry + Nav2. + +Launches in order: + 1. Gazebo Harmonic (via ros_gz_sim) with turtlebot3_world + 2. Robot state publisher + spawn TurtleBot3 + 3. Custom ros_gz_bridge WITHOUT the odom TF (FusionCore owns odom->base_footprint) + 4. FusionCore lifecycle node (configure -> activate automatically) + 5. icp_odometry using FusionCore's odom frame as scan-match initial guess + 6. rtabmap SLAM subscribing to icp_odometry output + 7. Nav2 using /fusion/odom + +Usage: + export TURTLEBOT3_MODEL=waffle + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py + + # Localization mode (requires existing map): + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true + + # Different world: + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house + +Note on TF ownership: + The standard turtlebot3_gazebo bridge forwards the DiffDrive TF to ROS, which + conflicts with FusionCore's odom->base_footprint. This demo uses a custom bridge + config (fusioncore_tb3_bridge.yaml) that suppresses the Gazebo TF entry. + FusionCore is the sole publisher of odom->base_footprint. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, + IncludeLaunchDescription, OpaqueFunction) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + if 'TURTLEBOT3_MODEL' not in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + tb3_model = os.environ['TURTLEBOT3_MODEL'] + pkg_tb3_gz = get_package_share_directory('turtlebot3_gazebo') + pkg_ros_gz = get_package_share_directory('ros_gz_sim') + pkg_nav2 = get_package_share_directory('nav2_bringup') + pkg_demos = get_package_share_directory('rtabmap_demos') + + world_name = LaunchConfiguration('world').perform(context) + world_file = os.path.join(pkg_tb3_gz, 'worlds', f'turtlebot3_{world_name}.world') + + # ── Gazebo server + client ──────────────────────────────────────────────── + gz_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')), + launch_arguments={ + 'gz_args': f'-r -s -v2 {world_file}', + 'on_exit_shutdown': 'true', + }.items(), + ) + gz_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')), + launch_arguments={'gz_args': '-g -v2', 'on_exit_shutdown': 'true'}.items(), + ) + + # ── Robot state publisher ───────────────────────────────────────────────── + robot_state_publisher = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_tb3_gz, 'launch', 'robot_state_publisher.launch.py')), + launch_arguments={'use_sim_time': 'true'}.items(), + ) + + # ── Spawn TurtleBot3 (entity only, no bridge) ───────────────────────────── + urdf_path = os.path.join(pkg_tb3_gz, 'models', + f'turtlebot3_{tb3_model}', 'model.sdf') + spawn_robot = Node( + package='ros_gz_sim', + executable='create', + arguments=[ + '-name', tb3_model, + '-file', urdf_path, + '-x', LaunchConfiguration('x_pose'), + '-y', LaunchConfiguration('y_pose'), + '-z', '0.01', + ], + output='screen', + ) + + # ── Custom bridge: all topics EXCEPT odom TF ────────────────────────────── + # FusionCore publishes odom->base_footprint; suppress the Gazebo DiffDrive TF. + bridge_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3_bridge.yaml') + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['--ros-args', '-p', f'config_file:={bridge_config}'], + output='screen', + ) + + # Camera image bridge (waffle only) + image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/image_raw'], + output='screen', + ) + + # ── FusionCore + icp_odometry + rtabmap ─────────────────────────────────── + fusioncore_icp = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_demos, 'launch', 'turtlebot3', 'fusioncore', + 'turtlebot3_fusioncore_icp.launch.py')), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ], + ) + + # ── Nav2 ────────────────────────────────────────────────────────────────── + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_nav2, 'launch', 'navigation_launch.py')), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', + 'turtlebot3_fusioncore_icp_nav2_params.yaml'])), + ], + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_nav2, 'launch', 'rviz_launch.py')), + ) + + set_gz_resource_path = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(pkg_tb3_gz, 'models'), + ) + + nodes = [ + set_gz_resource_path, + gz_server, + gz_client, + robot_state_publisher, + spawn_robot, + bridge, + fusioncore_icp, + nav2, + rviz, + ] + if tb3_model == 'waffle': + nodes.insert(6, image_bridge) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='world', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', + 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 Gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial X position in Gazebo.'), + + DeclareLaunchArgument( + 'y_pose', default_value='-0.5', + description='Initial Y position in Gazebo.'), + + OpaqueFunction(function=launch_setup), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index b9076449..37d59c89 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -20,11 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node +from launch.actions import OpaqueFunction -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') localization = LaunchConfiguration('localization') + max_ground_height = LaunchConfiguration('max_ground_height').perform(context) parameters={ 'frame_id':'base_footprint', @@ -36,7 +38,7 @@ def generate_launch_description(): 'Grid/3D':'false', # Use 2D occupancy 'Grid/RangeMax':'3', 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles - 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxGroundHeight': str(max_ground_height), # All points above 5 cm are obstacles 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -46,17 +48,7 @@ def generate_launch_description(): ('rgb/camera_info', '/camera/camera_info'), ('depth/image', '/camera/depth/image_raw')] - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='true', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'localization', default_value='false', - description='Launch in localization mode.'), - + return [ # Nodes to launch # SLAM mode: @@ -98,4 +90,23 @@ def generate_launch_description(): remappings=[('cloud', '/camera/cloud'), ('obstacles', '/camera/obstacles'), ('ground', '/camera/ground')]), - ]) + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'max_ground_height', default_value='0.05', + description='Maximum ground height, everything above is obstacle'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py index 733330fc..030a6741 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py @@ -20,12 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node +from launch.actions import OpaqueFunction - -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') localization = LaunchConfiguration('localization') + max_ground_height = LaunchConfiguration('max_ground_height').perform(context) parameters={ 'frame_id':'base_footprint', @@ -42,7 +43,7 @@ def generate_launch_description(): 'Grid/RangeMax':'3', 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map - 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxGroundHeight': str(max_ground_height), # All points above are obstacles 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) @@ -53,17 +54,7 @@ def generate_launch_description(): ('rgb/camera_info', '/camera/camera_info'), ('depth/image', '/camera/depth/image_raw')] - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'localization', default_value='false', - description='Launch in localization mode.'), - + return [ # Nodes to launch Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', @@ -109,4 +100,23 @@ def generate_launch_description(): remappings=[('cloud', '/camera/cloud'), ('obstacles', '/camera/obstacles'), ('ground', '/camera/ground')]), - ]) + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'max_ground_height', default_value='0.05', + description='Maximum ground height, everything above is obstacle'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py index e027c7e0..a66b55ec 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py @@ -76,7 +76,8 @@ def launch_setup(context, *args, **kwargs): # Visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": 'icp_odometry'}], remappings=remappings), ] diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py index ad14ca21..a4155c6a 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # Example: # $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py # @@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -77,20 +116,25 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + + max_ground_height = '0.05' + if ROS_DISTRO == 'jazzy': + max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ ('localization', LaunchConfiguration('localization')), - ('use_sim_time', 'true') + ('use_sim_time', 'true'), + ('max_ground_height', max_ground_height) ] ) return [ # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ 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 index 314ad3eb..311edd68 100644 --- 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 @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section (under same link) +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # Example: # $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py # @@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -77,6 +116,7 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ @@ -88,9 +128,8 @@ def launch_setup(context, *args, **kwargs): # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ 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 index 5a826cb6..d269639c 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section (under same link) +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans # hitting the robot itself # Example: @@ -30,9 +52,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -47,9 +72,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -62,13 +92,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -79,20 +118,25 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + + max_ground_height = '0.05' + if ROS_DISTRO == 'jazzy': + max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ ('localization', LaunchConfiguration('localization')), - ('use_sim_time', 'true') + ('use_sim_time', 'true'), + ('max_ground_height', max_ground_height) ] ) return [ # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py index 284b5b68..3fecc353 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py @@ -14,18 +14,22 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') pkg_nav2_bringup = get_package_share_directory( 'nav2_bringup') pkg_rtabmap_demos = get_package_share_directory( @@ -37,14 +41,26 @@ def launch_setup(context, *args, **kwargs): icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' if icp_odometry: # modified nav2 params to use icp_odom instead odom frame - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_scan_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] + ) else: - # original nav2 params - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + # original nav2 params + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] + ) + else: + # original nav2 params but with "enable_stamped_cmd_vel: True" + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_nav2_params.yaml'] + ) + # Paths nav2_launch = PathJoinSubstitution( @@ -53,56 +69,6 @@ def launch_setup(context, *args, **kwargs): [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) rtabmap_launch = PathJoinSubstitution( [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py']) - - # To use ICP odometry, we should increase clock rate of gazebo, we copied content of - # turtlebot3_gazebo/launch/turtlebot3_world.launch here - launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') - pkg_gazebo_ros = get_package_share_directory('gazebo_ros') - - world = os.path.join( - get_package_share_directory('turtlebot3_gazebo'), - 'worlds', - f'turtlebot3_{world_name}.world' - ) - - import tempfile - with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: - clock_override_file.write("---\n"+ - "gazebo:\n"+ - " ros__parameters:\n"+ - " publish_rate: 100.0") - - gzserver_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') - ), - launch_arguments={ - 'world': world, - 'params_file': clock_override_file.name}.items() - ) - - gzclient_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') - ) - ) - - robot_state_publisher_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') - ), - launch_arguments={'use_sim_time': 'true'}.items() - ) - - spawn_turtlebot_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') - ), - launch_arguments={ - 'x_pose': LaunchConfiguration('x_pose'), - 'y_pose': LaunchConfiguration('y_pose') - }.items() - ) nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), @@ -121,16 +87,83 @@ def launch_setup(context, *args, **kwargs): ('use_sim_time', 'true') ] ) + + # To use ICP odometry, we should increase clock rate of gazebo (humble), we copied content of + # turtlebot3_gazebo/launch/turtlebot3_world.launch here. + turtlebot3_nodes = [] + if ROS_DISTRO == 'humble': + pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + + import tempfile + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: + clock_override_file.write("---\n"+ + "gazebo:\n"+ + " ros__parameters:\n"+ + " publish_rate: 100.0") + + world = os.path.join( + pkg_turtlebot3_gazebo, + 'worlds', + f'turtlebot3_{world_name}.world' + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') + ), + launch_arguments={ + 'world': world, + 'params_file': clock_override_file.name}.items() + ) + + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') + ) + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_turtlebot3_gazebo, 'launch', 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': 'true'}.items() + ) + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_turtlebot3_gazebo, 'launch', 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': LaunchConfiguration('x_pose'), + 'y_pose': LaunchConfiguration('y_pose') + }.items() + ) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(pkg_turtlebot3_gazebo, 'models')) + turtlebot3_nodes = [ + gzserver_cmd, + gzclient_cmd, + robot_state_publisher_cmd, + spawn_turtlebot_cmd, + set_env_vars_resources + ] + else: + gazebo_launch = PathJoinSubstitution([pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world_name}.launch.py']) + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + turtlebot3_nodes = [gazebo] + return [ # Nodes to launch nav2, rviz, - rtabmap, - gzserver_cmd, - gzclient_cmd, - robot_state_publisher_cmd, - spawn_turtlebot_cmd - ] + rtabmap] + turtlebot3_nodes def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py index 85558ca8..63159b1d 100644 --- a/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py @@ -114,6 +114,7 @@ def generate_launch_description(): Node( condition=IfCondition(rtabmap_viz), package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": 'icp_odometry'}], remappings=remappings), ]) diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index d06df150..7c717b68 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -2,7 +2,7 @@ rtabmap_demos - 0.22.1 + 0.23.7 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/params/fusioncore_tb3.yaml b/rtabmap_demos/params/fusioncore_tb3.yaml new file mode 100644 index 00000000..7f8f476a --- /dev/null +++ b/rtabmap_demos/params/fusioncore_tb3.yaml @@ -0,0 +1,63 @@ +# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic) +# +# Platform: TurtleBot3 Waffle (simulated) +# IMU: MPU9250 (9-axis, magnetometer unreliable in sim) +# GPS: None (indoor / simulation) +# LiDAR: HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly) +# encoder2: rtabmap icp_odometry output on /rtabmap/icp_odometry +# +# Architecture: FusionCore (wheels + IMU) provides the odom frame. +# icp_odometry uses that odom frame as its initial scan-matching guess +# (guess_frame_id: odom). The ICP output feeds back into FusionCore +# as a second velocity source (encoder2). Each tightens the other. + +fusioncore: + ros__parameters: + base_frame: base_footprint + odom_frame: odom + publish_rate: 100.0 + publish.force_2d: true + + # Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu). + # Override to the TF frame that robot_state_publisher actually publishes. + imu.frame_id: "imu_link" + + # MPU9250: magnetometer disabled (unreliable in sim / near motors) + imu.has_magnetometer: false + # Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin. + # Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2. + # Gazebo injects lower noise than the real sensor, so tighter values + # reduce unnecessary filter uncertainty in sim without affecting real-hardware users + # (who should revert to the hardware spec values above). + imu.gyro_noise: 0.002 # rad/s (Gazebo sim-tuned; real MPU9250: 0.005) + imu.accel_noise: 0.02 # m/s2 (Gazebo sim-tuned; real MPU9250: 0.1) + imu.remove_gravitational_acceleration: false + + # Wheel odometry noise (TB3 differential drive, simulated) + encoder.vel_noise: 0.05 # m/s + encoder.yaw_noise: 0.02 # rad/s + + # ICP odometry as second velocity source + encoder2.topic: "/rtabmap/icp_odometry" + + outlier_rejection: true + outlier_threshold_imu: 15.09 + outlier_threshold_enc: 11.34 + + adaptive.imu: true + adaptive.encoder: true + adaptive.window: 50 + adaptive.alpha: 0.01 + + zupt.enabled: true + zupt.velocity_threshold: 0.08 # m/s: slightly loose for ICP jitter + zupt.angular_threshold: 0.05 # rad/s + zupt.noise_sigma: 0.01 + + ukf.q_position: 0.01 + ukf.q_orientation: 1.0e-9 + ukf.q_velocity: 0.1 + ukf.q_angular_vel: 0.1 + ukf.q_acceleration: 1.0 + ukf.q_gyro_bias: 1.0e-5 + ukf.q_accel_bias: 1.0e-5 diff --git a/rtabmap_demos/params/fusioncore_tb3_bridge.yaml b/rtabmap_demos/params/fusioncore_tb3_bridge.yaml new file mode 100644 index 00000000..fd3130df --- /dev/null +++ b/rtabmap_demos/params/fusioncore_tb3_bridge.yaml @@ -0,0 +1,47 @@ +# ros_gz_bridge config for the FusionCore + icp_odometry demo. +# +# Identical to turtlebot3_waffle_bridge.yaml EXCEPT the 'tf' entry is removed. +# FusionCore publishes odom -> base_footprint TF directly, so the DiffDrive +# plugin's TF must not be forwarded to avoid a competing transform. + +- ros_topic_name: "clock" + gz_topic_name: "clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS + +- ros_topic_name: "odom" + gz_topic_name: "odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + +- ros_topic_name: "cmd_vel" + gz_topic_name: "cmd_vel" + ros_type_name: "geometry_msgs/msg/TwistStamped" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ + +- ros_topic_name: "imu" + gz_topic_name: "imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +- ros_topic_name: "scan" + gz_topic_name: "scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +- ros_topic_name: "camera/camera_info" + gz_topic_name: "camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS diff --git a/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml new file mode 100644 index 00000000..838c80b3 --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml @@ -0,0 +1,287 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml new file mode 100644 index 00000000..43dce5ba --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml new file mode 100644 index 00000000..9c33bdb2 --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml @@ -0,0 +1,295 @@ +# Modified to use icp_odom frame +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: icp_odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: icp_odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml new file mode 100644 index 00000000..22ddc1c5 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml @@ -0,0 +1,280 @@ +# Nav2 parameters for FusionCore + icp_odometry TurtleBot3 demo. +# +# Key difference from stock nav2 params: +# - odom_topic: /fusion/odom (FusionCore output, not /odom or /odometry/filtered) +# - No AMCL: rtabmap handles the map -> odom transform via its SLAM output. + +bt_navigator: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_footprint + odom_topic: /fusion/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ['navigate_to_pose', 'navigate_through_poses'] + navigate_to_pose: + plugin: 'nav2_bt_navigator::NavigateToPoseNavigator' + navigate_through_poses: + plugin: 'nav2_bt_navigator::NavigateThroughPosesNavigator' + +controller_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + odom_topic: /fusion/odom + progress_checker_plugins: ['progress_checker'] + goal_checker_plugins: ['general_goal_checker'] + controller_plugins: ['FollowPath'] + progress_checker: + plugin: 'nav2_controller::SimpleProgressChecker' + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + general_goal_checker: + stateful: true + plugin: 'nav2_controller::SimpleGoalChecker' + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: 'nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController' + desired_linear_vel: 0.2 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_goal: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_feedforward_gain: 1.0 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: true + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ['obstacle_layer', 'inflation_layer'] + obstacle_layer: + plugin: 'nav2_costmap_2d::ObstacleLayer' + enabled: true + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: 'LaserScan' + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: 'nav2_costmap_2d::InflationLayer' + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: true + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ['static_layer', 'obstacle_layer', 'inflation_layer'] + static_layer: + plugin: 'nav2_costmap_2d::StaticLayer' + map_subscribe_transient_local: true + obstacle_layer: + plugin: 'nav2_costmap_2d::ObstacleLayer' + enabled: true + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: 'LaserScan' + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: 'nav2_costmap_2d::InflationLayer' + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + +planner_server: + ros__parameters: + use_sim_time: true + expected_planner_frequency: 20.0 + planner_plugins: ['GridBased'] + GridBased: + plugin: 'nav2_navfn_planner::NavfnPlanner' + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + smoother_plugins: ['simple_smoother'] + simple_smoother: + plugin: 'nav2_smoother::SimpleSmoother' + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: true + +behavior_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ['spin', 'backup', 'drive_on_heading', 'assisted_teleop', 'wait'] + spin: + plugin: 'nav2_behaviors::Spin' + backup: + plugin: 'nav2_behaviors::BackUp' + drive_on_heading: + plugin: 'nav2_behaviors::DriveOnHeading' + wait: + plugin: 'nav2_behaviors::Wait' + assisted_teleop: + plugin: 'nav2_behaviors::AssistedTeleop' + local_frame: odom + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +velocity_smoother: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + smoothing_frequency: 20.0 + scale_velocities: false + feedback: 'OPEN_LOOP' + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /fusion/odom + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + base_frame_id: base_footprint + odom_frame_id: odom + cmd_vel_in_topic: cmd_vel_smoothed + cmd_vel_out_topic: cmd_vel + state_topic: collision_monitor_state + transform_error_pub_topic: transform_error + polygons: ['FootprintApproach'] + FootprintApproach: + type: polygon + action_type: approach + footprint_topic: local_costmap/published_footprint + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: false + enabled: true + observation_sources: ['scan'] + scan: + type: scan + topic: /scan + min_height: 0.15 + max_height: 2.0 + enabled: true + + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_footprint" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 \ No newline at end of file diff --git a/rtabmap_demos/params/turtlebot3_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_nav2_params.yaml new file mode 100644 index 00000000..0a861836 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_nav2_params.yaml @@ -0,0 +1,421 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + costmap_update_timeout: 0.30 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.7 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + +velocity_smoother: + ros__parameters: + enable_stamped_cmd_vel: True + smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml index 838c80b3..5d32f405 100644 --- a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml @@ -1,85 +1,44 @@ # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -200,7 +206,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -211,19 +216,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True -map_server: +planner_server: ros__parameters: - use_sim_time: True - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -233,55 +241,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml index 43dce5ba..cb833425 100644 --- a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml @@ -1,85 +1,44 @@ # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -210,7 +216,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -221,23 +226,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -247,55 +251,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true \ No newline at end of file diff --git a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml index 9c33bdb2..1acddc5b 100644 --- a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml @@ -1,85 +1,44 @@ -# Modified to use icp_odom frame +# Using icp_odom TF instead of odom bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: icp_odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -190,7 +196,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -215,23 +220,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -241,55 +245,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: icp_odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: icp_odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "icp_odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "icp_odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "icp_odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/rtabmap_examples/config/multi_rgbd_inertial_dataset.ini b/rtabmap_examples/config/multi_rgbd_inertial_dataset.ini new file mode 100644 index 00000000..47189c4e --- /dev/null +++ b/rtabmap_examples/config/multi_rgbd_inertial_dataset.ini @@ -0,0 +1,453 @@ +[Core] +Rtabmap\WorkingDirectory=/home/vscode/.ros + +[Gui] +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0\0\a\x80\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10) +DepthCalibrationDialog\bin_depth=2 +DepthCalibrationDialog\bin_height=6 +DepthCalibrationDialog\bin_width=8 +DepthCalibrationDialog\cone_radius=0.02 +DepthCalibrationDialog\cone_stddev_thresh=0.1 +DepthCalibrationDialog\decimation=1 +DepthCalibrationDialog\laser_scan=false +DepthCalibrationDialog\max_depth=3.5 +DepthCalibrationDialog\max_model_depth=10 +DepthCalibrationDialog\min_depth=0 +DepthCalibrationDialog\smoothing=1 +DepthCalibrationDialog\voxel=0.01 +ExportBundlerDialog\exportPoints=false +ExportBundlerDialog\laplacianThr=0 +ExportBundlerDialog\maxAngularSpeed=0 +ExportBundlerDialog\maxLinearSpeed=0 +ExportBundlerDialog\sba_iterations=20 +ExportBundlerDialog\sba_rematch_features=true +ExportBundlerDialog\sba_type=0 +ExportBundlerDialog\sba_variance=1 +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_samples=0 +ExportCloudsDialog\assemble_voxel=0.01 +ExportCloudsDialog\bilateral=false +ExportCloudsDialog\bilateral_sigma_r=0.1 +ExportCloudsDialog\bilateral_sigma_s=10 +ExportCloudsDialog\binary=true +ExportCloudsDialog\cam_proj=false +ExportCloudsDialog\cam_proj_decimation=1 +ExportCloudsDialog\cam_proj_distance_policy=true +ExportCloudsDialog\cam_proj_export_format=0 +ExportCloudsDialog\cam_proj_keep_points=false +ExportCloudsDialog\cam_proj_mask= +ExportCloudsDialog\cam_proj_max_angle=0 +ExportCloudsDialog\cam_proj_max_depth_error=0 +ExportCloudsDialog\cam_proj_max_distance=0 +ExportCloudsDialog\cam_proj_recolor_points=true +ExportCloudsDialog\cam_proj_roi_ratios=0.0 0.0 0.0 0.0 +ExportCloudsDialog\cputsdf_flattenRadius=0.005 +ExportCloudsDialog\cputsdf_minWeight=0 +ExportCloudsDialog\cputsdf_randomSplit=1 +ExportCloudsDialog\cputsdf_resolution=0.01 +ExportCloudsDialog\cputsdf_size=12 +ExportCloudsDialog\cputsdf_truncNeg=0.03 +ExportCloudsDialog\cputsdf_truncPos=0.03 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_min_neighbors=5 +ExportCloudsDialog\filtering_radius=0 +ExportCloudsDialog\frame=0 +ExportCloudsDialog\from_depth=true +ExportCloudsDialog\gain=false +ExportCloudsDialog\gain_beta=10 +ExportCloudsDialog\gain_full=false +ExportCloudsDialog\gain_overlap=0 +ExportCloudsDialog\gain_radius=0.02 +ExportCloudsDialog\gain_rgb=true +ExportCloudsDialog\intensity_colormap=0 +ExportCloudsDialog\mesh=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_clean=true +ExportCloudsDialog\mesh_color_radius=0.05 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_dense_strategy=1 +ExportCloudsDialog\mesh_max_polygons=0 +ExportCloudsDialog\mesh_min_cluster_size=0 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_radius=0.2 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_textureBlending=true +ExportCloudsDialog\mesh_textureBlendingDecimation=0 +ExportCloudsDialog\mesh_textureBrightnessConstrastRatioHigh=5 +ExportCloudsDialog\mesh_textureBrightnessConstrastRatioLow=0 +ExportCloudsDialog\mesh_textureCameraFiltering=false +ExportCloudsDialog\mesh_textureCameraFilteringAngle=30 +ExportCloudsDialog\mesh_textureCameraFilteringLaplacian=0 +ExportCloudsDialog\mesh_textureCameraFilteringRadius=0 +ExportCloudsDialog\mesh_textureCameraFilteringVel=0 +ExportCloudsDialog\mesh_textureCameraFilteringVelRad=0 +ExportCloudsDialog\mesh_textureDistanceToCamPolicy=false +ExportCloudsDialog\mesh_textureExposureFusion=false +ExportCloudsDialog\mesh_textureFormat=0 +ExportCloudsDialog\mesh_textureMaxAngle=0 +ExportCloudsDialog\mesh_textureMaxCount=1 +ExportCloudsDialog\mesh_textureMaxDepthError=0 +ExportCloudsDialog\mesh_textureMaxDistance=3 +ExportCloudsDialog\mesh_textureMinCluster=50 +ExportCloudsDialog\mesh_textureMultiband=false +ExportCloudsDialog\mesh_textureMultibandAngleHardThr=90 +ExportCloudsDialog\mesh_textureMultibandBestScoreThr=0.1 +ExportCloudsDialog\mesh_textureMultibandDownScale=2 +ExportCloudsDialog\mesh_textureMultibandFillHoles=false +ExportCloudsDialog\mesh_textureMultibandForceVisible=false +ExportCloudsDialog\mesh_textureMultibandNbContrib=1 5 10 0 +ExportCloudsDialog\mesh_textureMultibandPadding=5 +ExportCloudsDialog\mesh_textureMultibandUnwrap=0 +ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0 +ExportCloudsDialog\mesh_textureSize=6 +ExportCloudsDialog\mesh_textureVertexColorPolicy=0 +ExportCloudsDialog\mesh_triangle_size=1 +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_dilation_iterations=1 +ExportCloudsDialog\mls_dilation_voxel_size=0.005 +ExportCloudsDialog\mls_output_voxel_size=0 +ExportCloudsDialog\mls_point_density=10 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_radius=0.04 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0.005 +ExportCloudsDialog\nodes_filtering=false +ExportCloudsDialog\nodes_filtering_xmax=0 +ExportCloudsDialog\nodes_filtering_xmin=0 +ExportCloudsDialog\nodes_filtering_ymax=0 +ExportCloudsDialog\nodes_filtering_ymin=0 +ExportCloudsDialog\nodes_filtering_zmax=0 +ExportCloudsDialog\nodes_filtering_zmin=0 +ExportCloudsDialog\normals_ground_normals_up=0 +ExportCloudsDialog\normals_k=20 +ExportCloudsDialog\normals_radius=0 +ExportCloudsDialog\openchisel_carving_dist_m=0.05 +ExportCloudsDialog\openchisel_chunk_size_x=16 +ExportCloudsDialog\openchisel_chunk_size_y=16 +ExportCloudsDialog\openchisel_chunk_size_z=16 +ExportCloudsDialog\openchisel_far_plane_dist=1.1 +ExportCloudsDialog\openchisel_integration_weight=1 +ExportCloudsDialog\openchisel_merge_vertices=true +ExportCloudsDialog\openchisel_near_plane_dist=0.05 +ExportCloudsDialog\openchisel_truncation_constant=0.001504 +ExportCloudsDialog\openchisel_truncation_linear=0.00152 +ExportCloudsDialog\openchisel_truncation_quadratic=0.0019 +ExportCloudsDialog\openchisel_truncation_scale=10 +ExportCloudsDialog\openchisel_use_voxel_carving=false +ExportCloudsDialog\pipeline=1 +ExportCloudsDialog\poisson_depth=0 +ExportCloudsDialog\poisson_iso=8 +ExportCloudsDialog\poisson_manifold=true +ExportCloudsDialog\poisson_minDepth=5 +ExportCloudsDialog\poisson_outputPolygons=false +ExportCloudsDialog\poisson_pointWeight=4 +ExportCloudsDialog\poisson_polygon_size=0.03 +ExportCloudsDialog\poisson_samples=1 +ExportCloudsDialog\poisson_scale=1.1 +ExportCloudsDialog\poisson_solver=8 +ExportCloudsDialog\regenerate=false +ExportCloudsDialog\regenerate_ceiling=0 +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_distortion_model= +ExportCloudsDialog\regenerate_edge_bleeding_error=0 +ExportCloudsDialog\regenerate_fill_error=2 +ExportCloudsDialog\regenerate_fill_size=0 +ExportCloudsDialog\regenerate_floor=0 +ExportCloudsDialog\regenerate_footprint_height=0 +ExportCloudsDialog\regenerate_footprint_length=0 +ExportCloudsDialog\regenerate_footprint_width=0 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\regenerate_min_depth=0 +ExportCloudsDialog\regenerate_min_depth_confidence=0 +ExportCloudsDialog\regenerate_offaxis_filtering=false +ExportCloudsDialog\regenerate_offaxis_filtering_angle=10 +ExportCloudsDialog\regenerate_offaxis_filtering_neg_x=true +ExportCloudsDialog\regenerate_offaxis_filtering_neg_y=true +ExportCloudsDialog\regenerate_offaxis_filtering_neg_z=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_x=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_y=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_z=true +ExportCloudsDialog\regenerate_roi=0.0 0.0 0.0 0.0 +ExportCloudsDialog\regenerate_scan_decimation=1 +ExportCloudsDialog\regenerate_scan_max_range=0 +ExportCloudsDialog\regenerate_scan_min_range=0 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\subtract_point_angle=0 +ExportCloudsDialog\subtract_point_radius=0.02 +Figures\counts= +Figures\curves= +Figures\thresholds= +General\beep=false +General\cloudCeilingHeight=0 +General\cloudFiltering=false +General\cloudFilteringAngle=30 +General\cloudFilteringRadius=0.1 +General\cloudFloorHeight=0 +General\cloudNoiseMinNeighbors=5 +General\cloudNoiseRadius=0 +General\cloudVoxel=0 +General\cloudsKept=true +General\colorScheme0=0 +General\colorScheme1=0 +General\colorSchemeScan0=0 +General\colorSchemeScan1=0 +General\decimation0=8 +General\decimation1=4 +General\depthConf0=0 +General\depthConf1=0 +General\downsamplingScan0=1 +General\downsamplingScan1=1 +General\elevationMapShown=0 +General\figure_cache=true +General\figure_time=true +General\gravityLength0=1 +General\gravityLength1=1 +General\gravityShown0=false +General\gravityShown1=true +General\gridMapOpacity=0.75 +General\gridMapShown=false +General\gridUIResolution=0 +General\gtAlign=true +General\imageHighestHypShown=false +General\imageRejectedShown=true +General\imagesKept=true +General\landmarkSize=0 +General\localizationsGraphView=false +General\localizationsGraphViewOdomCache=false +General\loggerEventLevel=3 +General\loggerLevel=2 +General\loggerPauseLevel=3 +General\loggerPrintThreadId=false +General\loggerPrintTime=true +General\loggerType=1 +General\maxDepth0=5 +General\maxDepth1=0 +General\maxRange0=0 +General\maxRange1=0 +General\meshing=false +General\meshing_angle=15 +General\meshing_quad=true +General\meshing_texture=false +General\meshing_triangle_size=2 +General\minDepth0=0 +General\minDepth1=0 +General\minRange0=0 +General\minRange1=0 +General\missingRepublished=true +General\noFiltering=true +General\nochangeGraphView=false +General\normalKSearch=10 +General\normalRadiusSearch=0 +General\notifyNewGlobalPath=false +General\octomap=false +General\octomap_2dgrid=true +General\octomap_3dmap=true +General\octomap_depth=16 +General\octomap_point_size=5 +General\octomap_rendering_type=0 +General\odomDisabled=false +General\odomF2MGravitySigma=-1 +General\odomOnlyInliersShown=false +General\odomQualityThr=50 +General\odomRegistration=3 +General\opacity0=1 +General\opacity1=0.75 +General\opacityScan0=1 +General\opacityScan1=0.5 +General\posteriorGraphView=true +General\ptSize0=1 +General\ptSize1=2 +General\ptSizeFeatures0=3 +General\ptSizeFeatures1=3 +General\ptSizeScan0=1 +General\ptSizeScan1=2 +General\roiRatios0=0.0 0.0 0.0 0.0 +General\roiRatios1=0.0 0.0 0.0 0.0 +General\scanCeilingHeight=0 +General\scanFloorHeight=0 +General\scanNormalKSearch=0 +General\scanNormalRadiusSearch=0 +General\showClouds0=true +General\showClouds1=false +General\showFeatures0=false +General\showFeatures1=true +General\showFrames=false +General\showFrustums0=false +General\showFrustums1=false +General\showGraphs=true +General\showIMUAcc=false +General\showIMUGravity=false +General\showLabels=false +General\showLandmarks=true +General\showScans0=true +General\showScans1=true +General\subtractFiltering=false +General\subtractFilteringAngle=0 +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\verticalLayoutUsed=true +General\voxelSizeScan0=0 +General\voxelSizeScan1=0 +General\wordsGraphView=false +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\x32\0\0\0\x1b\0\0\x6\xf2\0\0\x3\xf\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf) +MainWindow\maximized=false +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x3\\\0\0\x2\x95\xfc\x2\0\0\0\x3\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\xc1\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0;\0\0\x1\xaa\0\0\0\x37\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\x1\xeb\0\0\0\xe5\0\0\0\x13\0\xff\xff\xff\0\0\0\x1\0\0\x3_\0\0\x2\x95\xfc\x2\0\0\0\x4\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0;\0\0\x2\x95\0\0\0\xdb\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf0\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x2\0\0\x4\xf2\0\0\x1\x39\0\0\x2}\0\0\x1\x90\xfb\0\0\0\x34\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0u\0l\0t\0i\0S\0\x65\0s\0s\0i\0o\0n\0L\0o\0\x63\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x13\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1(\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\x88\0\0\0\0\0\0\x2\x95\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\status_bar=false +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\cluster_radius=1 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\inter_session=true +PostProcessingDialog\intra_session=true +PostProcessingDialog\iterations=5 +PostProcessingDialog\refine_lc=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_rematch_features=true +PostProcessingDialog\sba_type=1 +PostProcessingDialog\sba_variance=1 +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x1\xa8\xff\xff\xff\xf6\0\0\x5{\0\0\x3\xb7\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7\0\0\0\0\0\0\0\0\a\x80\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\ensure_frame_visible=1 +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\global_path_visible=true +graphicsView_graphView\gps_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\x80\x80\0\0) +graphicsView_graphView\gps_graph_visible=true +graphicsView_graphView\graph_visible=true +graphicsView_graphView\grid_visible=true +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\gt_graph_visible=true +graphicsView_graphView\highlighting_color_0=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\highlighting_color_1=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\link_width=0 +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\local_path_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\min_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\node_odom_cache_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\0\0\0\0) +graphicsView_graphView\node_radius=0.009999999776482582 +graphicsView_graphView\node_visible=true +graphicsView_graphView\odom_cache_overlay=true +graphicsView_graphView\orientation_ENU=false +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\view_plane=0 +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_loopClosure\alpha=100 +imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_loopClosure\colormap=2 +imageView_loopClosure\colormap_camera_frame=true +imageView_loopClosure\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_loopClosure\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_loopClosure\confidence_shown=false +imageView_loopClosure\depth_shown=false +imageView_loopClosure\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_loopClosure\features_shown=true +imageView_loopClosure\features_size=0 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_loopClosure\graphics_view_scale_to_height=false +imageView_loopClosure\image_shown=true +imageView_loopClosure\lines_shown=true +imageView_loopClosure\lines_width=0 +imageView_loopClosure\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_loopClosure\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +imageView_odometry\alpha=200 +imageView_odometry\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_odometry\colormap=2 +imageView_odometry\colormap_camera_frame=true +imageView_odometry\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_odometry\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_odometry\confidence_shown=false +imageView_odometry\depth_shown=false +imageView_odometry\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_odometry\features_shown=true +imageView_odometry\features_size=0 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +imageView_odometry\graphics_view_scale_to_height=false +imageView_odometry\image_shown=true +imageView_odometry\lines_shown=true +imageView_odometry\lines_width=0 +imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +imageView_source\alpha=100 +imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\colormap=2 +imageView_source\colormap_camera_frame=true +imageView_source\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_source\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_source\confidence_shown=false +imageView_source\depth_shown=false +imageView_source\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_source\features_shown=true +imageView_source\features_size=0 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_source\graphics_view_scale_to_height=false +imageView_source\image_shown=true +imageView_source\lines_shown=true +imageView_source\lines_width=0 +imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +multisession_imageview\alpha=100 +multisession_imageview\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +multisession_imageview\colormap=2 +multisession_imageview\colormap_camera_frame=true +multisession_imageview\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +multisession_imageview\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +multisession_imageview\confidence_shown=false +multisession_imageview\depth_shown=false +multisession_imageview\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +multisession_imageview\features_shown=true +multisession_imageview\features_size=0 +multisession_imageview\graphics_view=false +multisession_imageview\graphics_view_scale=true +multisession_imageview\graphics_view_scale_to_height=false +multisession_imageview\image_shown=true +multisession_imageview\lines_shown=true +multisession_imageview\lines_width=0 +multisession_imageview\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +multisession_imageview\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_axis_shown=true +widget_cloudViewer\camera_focal=@Variant(\0\0\0T8\xb6\0\0\x37s\0\0\xb5\xd5\0\0) +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0\xc0\x9b\x36@\x80\xf7\xea\x41\x17p\xe) +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0) +widget_cloudViewer\color_range_inverted=0 +widget_cloudViewer\color_range_max=0 +widget_cloudViewer\color_range_min=0 +widget_cloudViewer\coordinate_frame_scale=1 +widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0) +widget_cloudViewer\frustum_shown=true +widget_cloudViewer\grid=false +widget_cloudViewer\grid_cell_count=50 +widget_cloudViewer\grid_cell_size=1 +widget_cloudViewer\intensity_max=100 +widget_cloudViewer\intensity_rainbow_colormap=false +widget_cloudViewer\intensity_red_colormap=true +widget_cloudViewer\normals=false +widget_cloudViewer\normals_scale=0.20000000298023224 +widget_cloudViewer\normals_step=1 +widget_cloudViewer\rendering_rate=5 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 diff --git a/rtabmap_examples/config/multi_rgbd_inertial_dataset_front.yaml b/rtabmap_examples/config/multi_rgbd_inertial_dataset_front.yaml new file mode 100644 index 00000000..8cfe3391 --- /dev/null +++ b/rtabmap_examples/config/multi_rgbd_inertial_dataset_front.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_FRONT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.1610, 0., 329.5844, 0., + 319.0881, 182.1654, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0535, 0.0589, -0.0176, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.1610, 0., 329.5844, 0., 0., + 319.0881, 182.1654, 0., 0., 0., 1., + 0. ] \ No newline at end of file diff --git a/rtabmap_examples/config/multi_rgbd_inertial_dataset_left.yaml b/rtabmap_examples/config/multi_rgbd_inertial_dataset_left.yaml new file mode 100644 index 00000000..cfa257c4 --- /dev/null +++ b/rtabmap_examples/config/multi_rgbd_inertial_dataset_left.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_LEFT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.1506, 0., 321.8367, 0., + 318.9474, 180.0959, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0556, 0.0576, -0.0174, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.1506, 0., 321.8367, 0., 0., + 318.9474, 180.0959, 0., 0., 0., 1., + 0. ] diff --git a/rtabmap_examples/config/multi_rgbd_inertial_dataset_rear.yaml b/rtabmap_examples/config/multi_rgbd_inertial_dataset_rear.yaml new file mode 100644 index 00000000..c38bbb90 --- /dev/null +++ b/rtabmap_examples/config/multi_rgbd_inertial_dataset_rear.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_REAR +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.3360, 0., 323.9265, 0., + 319.1947, 181.9133, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0534, 0.0542, -0.0155, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.3360, 0., 323.9265, 0., 0., + 319.1947, 181.9133, 0., 0., 0., 1., + 0. ] \ No newline at end of file diff --git a/rtabmap_examples/config/multi_rgbd_inertial_dataset_right.yaml b/rtabmap_examples/config/multi_rgbd_inertial_dataset_right.yaml new file mode 100644 index 00000000..8cf33717 --- /dev/null +++ b/rtabmap_examples/config/multi_rgbd_inertial_dataset_right.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_RIGHT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 320.7208, 0., 325.4179, 0., + 320.7359, 184.8089, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0531, 0.0549, -0.0166, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 320.7208, 0., 325.4179, 0., 0., + 320.7359, 184.8089, 0., 0., 0., 1., + 0. ] diff --git a/rtabmap_examples/launch/depthai.launch.py b/rtabmap_examples/launch/depthai.launch.py index 8918bba0..41b077d6 100644 --- a/rtabmap_examples/launch/depthai.launch.py +++ b/rtabmap_examples/launch/depthai.launch.py @@ -1,8 +1,14 @@ # Requirements: # A OAK-D camera -# Install depthai-ros package (https://github.com/luxonis/depthai-ros) +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! # Example: # $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed IR-D images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. import os diff --git a/rtabmap_examples/launch/depthai_color.launch.py b/rtabmap_examples/launch/depthai_color.launch.py new file mode 100644 index 00000000..6fc0202b --- /dev/null +++ b/rtabmap_examples/launch/depthai_color.launch.py @@ -0,0 +1,81 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! +# Example: +# $ ros2 launch rtabmap_examples depthai_color.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed RGB-D images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters=[{'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False}] + + sync_parameters=[{'approx_sync':True, + 'approx_sync_max_interval':0.005}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'enableRviz': 'false', + 'rgbResolution': '1080p', + 'rgbScaleNumerator': '2', # Convert to 720p (same size than depth) + 'rgbScaleDinominator': '3'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=sync_parameters, + remappings=[('rgb/image', '/color/image'), + ('rgb/camera_info', '/color/camera_info'), + ('depth/image', '/stereo/depth')]), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ]) diff --git a/rtabmap_examples/launch/depthai_stereo.launch.py b/rtabmap_examples/launch/depthai_stereo.launch.py new file mode 100644 index 00000000..a5479607 --- /dev/null +++ b/rtabmap_examples/launch/depthai_stereo.launch.py @@ -0,0 +1,90 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! +# +# Issue: To have compatible stereo camera info with rtabmap (P[0,3] should be positive on left camera info or negative in right camera info), +# we should add the following line here (https://github.com/luxonis/depthai-ros/blob/887248d72cc6b9515793f828645346408b9cad47/depthai_examples/src/stereo_inertial_publisher.cpp#L593) +# so that left camera info has a positive P[0,3] instead of negative to correctly compute the baseline: +# +# leftCameraInfo.p[3] *=-1; +# +# Example: +# $ ros2 launch rtabmap_examples depthai_stereo.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed stereo IR images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters={'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + sync_parameters=[{'approx_sync':True, + 'approx_sync_max_interval':0.005}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'depth_aligned': 'false', # not color mode + 'enableRviz': 'false', + 'monoResolution': '400p'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='stereo_sync', output='screen', + parameters=sync_parameters, + remappings=[('left/image', '/left/image_rect'), + ('left/camera_info', '/left/camera_info'), + ('right/image', '/right/image_rect'), + ('right/camera_info', '/right/camera_info')]), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[parameters], + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=remappings) + ]) diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index 8c9f31ca..71b08fb8 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -62,7 +62,8 @@ def generate_launch_description(): # Nodes to launch Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', - parameters=[parameters], + parameters=[parameters, + { 'always_process_most_recent_frame':True}], remappings=remappings), Node( @@ -82,7 +83,8 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], remappings=remappings), # Image rectification and publishing synchronized camera_info @@ -103,15 +105,13 @@ def generate_launch_description(): namespace='stereo_camera'), Node( - package='image_proc', executable='image_proc', output='screen', + package='image_proc', executable='rectify_node', output='screen', remappings=[ - ('image_raw', '/cam0/image_raw'), ('image', '/cam0/image_raw')], namespace='stereo_camera/left'), Node( - package='image_proc', executable='image_proc', output='screen', + package='image_proc', executable='rectify_node', output='screen', remappings=[ - ('image_raw', '/cam1/image_raw'), ('image', '/cam1/image_raw')], namespace='stereo_camera/right'), diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index 6ddee09a..f72e82b9 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -154,7 +154,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) ] diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 9c217640..1fd3ef68 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -173,7 +173,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): # Just for visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', viz_topic)]) ] diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index 23da663f..accc9da3 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -201,7 +201,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): # Just for visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', viz_topic)]) ] diff --git a/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch.py b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch.py new file mode 100644 index 00000000..589f7653 --- /dev/null +++ b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch.py @@ -0,0 +1,162 @@ +# +# Example launch file to run VSLAM on this dataset: https://github.com/seungsang07/multi-rgbd-inertial-dataset +# +# Requirement(s): +# * RTAB-Map should be built with OpenGV support. +# +# To convert ROS1 bags to ROS2 (https://docs.openvins.com/dev-ros1-to-ros2.html): +# sudo pip install rosbags +# rosbags-convert --src Indoor.bag --dst Indoor +# +# Usage: +# ros2 launch rtabmap_examples multi_rgbd_inertial_dataset.launch.py +# ros2 bag play Indoor/Indoor.db3 --clock +# +# Note(s): +# * Communication performance could be improved using the composable nodes of these nodes instead, +# but we are using nodes here to better understand what is going on with rqt_graph. +# +# To get RMSE after the run (using https://github.com/MichaelGrupp/evo): +# rtabmap-export --poses --pose_format 10 ~/.ros/rtabmap.db +# evo_ape tum -a -p --plot_mode xy ~/Downloads/GroundTruth/gt_indoor.txt ~/.ros/rtabmap_poses.txt +# + +from launch import LaunchDescription, LaunchContext +from launch_ros.actions import SetParameter +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + +def make_yaml_to_camera_info_node(camera): + # the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them + return Node( + package='rtabmap_util', executable='yaml_to_camera_info.py', name=f'yaml_to_camera_info_{camera}', output='screen', + parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), f'/config/multi_rgbd_inertial_dataset_{camera}.yaml']}], + remappings=[ + ('image', 'color/image_raw'), + ('camera_info', 'color/camera_info')], + namespace=f'camera_{camera}') + +def make_rgbd_sync_node(camera): + # synchronize topics of each camera together + return Node( + package='rtabmap_sync', executable='rgbd_sync', name=f'rgbd_sync_{camera}', output="screen", + parameters=[{"approx_sync": False}], + remappings=[ + ("rgb/image", 'color/image_raw'), + ("depth/image", 'aligned_depth_to_color/image_raw'), + ("rgb/camera_info", 'color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace=f'camera_{camera}') + +def make_static_transform_publisher_node(x, y, z, yaw, pitch, roll, parent_frame, child_frame): + return Node( + package='tf2_ros', executable='static_transform_publisher', name=f"static_transform_publisher_{parent_frame}_{child_frame}", output='screen', + arguments=['--frame-id', parent_frame, + '--child-frame-id', child_frame, + '--x', str(x), + '--y', str(y), + '--z', str(z), + '--yaw', str(yaw), + '--pitch', str(pitch), + '--roll', str(roll)]) + +def launch_setup(context: LaunchContext, *args, **kwargs): + + use_imu = LaunchConfiguration('use_imu').perform(context) + use_imu = use_imu == 'true' or use_imu == 'True' + + # Synchronize all cameras together + rgbdx_sync_node = Node( + package='rtabmap_sync', executable='rgbdx_sync', output="screen", + parameters=[{ + "rgbd_cameras": 4, + "approx_sync": True, + "approx_sync_max_interval": 0.015}], + remappings=[ + ("rgbd_image0", '/camera_left/rgbd_image'), + ("rgbd_image1", '/camera_front/rgbd_image'), + ("rgbd_image2", '/camera_right/rgbd_image'), + ("rgbd_image3", '/camera_rear/rgbd_image')], + namespace='rtabmap' + ) + + # RGB-D odometry + remappings = [] + if use_imu: + remappings = [("imu", '/imu')] + rgbd_odometry_node = Node( + package='rtabmap_odom', executable='rgbd_odometry', output="screen", + parameters=[{ + "frame_id": 'base_link', + "rgbd_cameras": 0, # make it subscribe to rgbd_images topic from rgbdx_sync + "subscribe_rgbd": True, + "wait_imu_to_init": use_imu}], + remappings=remappings, + namespace='rtabmap' + ) + + # SLAM + remappings=[("sensor_data", 'odom_sensor_data/raw')] + if(use_imu): + remappings.append(('imu', '/imu')) + slam_node = Node( + package='rtabmap_slam', executable='rtabmap', output="screen", + parameters=[{ + "subscribe_sensor_data": True, + "frame_id": 'base_link', + "approx_sync": False, + "Grid/3D": 'false', + "Grid/RayTracing": 'true', + "Grid/NormalsSegmentation": 'false', + "Grid/MaxGroundHeight": '0.05', + "Rtabmap/CreateIntermediateNodes": 'true' # Only to record all odometry poses for trajectory evaluation purpose + }], + remappings=remappings, + arguments=["--delete_db_on_start"], + namespace='rtabmap' + ) + + # Visualization + viz_node = Node( + package='rtabmap_viz', executable='rtabmap_viz', output="screen", + parameters=[{ + "subscribe_sensor_data": True, + "frame_id": 'base_link', + "approx_sync": False, + "subscribe_odom_info": True + }], + remappings=[("sensor_data", 'odom_sensor_data/raw')], + arguments=["-d", [FindPackageShare('rtabmap_examples'), '/config/multi_rgbd_inertial_dataset.ini']], + namespace='rtabmap' + ) + + return [ + make_yaml_to_camera_info_node('left'), + make_rgbd_sync_node('left'), + make_yaml_to_camera_info_node('front'), + make_rgbd_sync_node('front'), + make_yaml_to_camera_info_node('right'), + make_rgbd_sync_node('right'), + make_yaml_to_camera_info_node('rear'), + make_rgbd_sync_node('rear'), + # The dataset doesn't provide /tf or /tf_static for the extrinsics between imu and the cameras, so we add them here + make_static_transform_publisher_node(0., 0., 0.22, 3.1415926, 0., 0., 'base_link', 'imu_link'), + make_static_transform_publisher_node(-0.099307, -0.208806, 0.024309, 3.108592, -0.051480, -1.592415, 'imu_link', 'camera_left_color_optical_frame'), + make_static_transform_publisher_node(-0.435392, 0.022256, 0.053441, 1.532258, -0.007768, -1.580204, 'imu_link', 'camera_front_color_optical_frame'), + make_static_transform_publisher_node(-0.063987, 0.212966, 0.032071, -0.010030, 0.021977, -1.553814, 'imu_link', 'camera_right_color_optical_frame'), + make_static_transform_publisher_node(0.178893, -0.006307, 0.017677, -1.606156, 0.027545, -1.587179, 'imu_link', 'camera_rear_color_optical_frame'), + make_static_transform_publisher_node(0.045872, -0.026775, 0.284806, 3.141063, -0.014869, -0.014969, 'imu_link', 'os_sensor'), + rgbdx_sync_node, + rgbd_odometry_node, + slam_node, + viz_node + ] + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('use_imu', default_value='false', description='Use IMU'), + SetParameter(name='use_sim_time', value=True), + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index ae4e2edb..c0eb257f 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -37,6 +37,15 @@ def generate_launch_description(): # Make sure IR emitter is enabled SetParameter(name='depth_module.emitter_enabled', value=1), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + # Launch camera driver IncludeLaunchDescription( @@ -55,13 +64,14 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=parameters, remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py new file mode 100644 index 00000000..3c1c0111 --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py @@ -0,0 +1,130 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_color_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_color.launch.py: the +# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# As in the non-composed example, the color stream is used as RGB and paired +# with the depth aligned to color (align_depth.enable), with the IR emitter on. +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'align_depth.enable': True, + 'enable_sync': True, + 'rgb_camera.profile': '640x360x30', + 'depth_module.emitter_enabled': 1}], # Make sure IR emitter is enabled + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # RGB-D odometry (color + depth aligned to color) + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_combined.launch.py b/rtabmap_examples/launch/realsense_d435i_combined.launch.py new file mode 100644 index 00000000..89efdffc --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_combined.launch.py @@ -0,0 +1,106 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_combined.launch.py +# +# Description: In this example, we feed visual odometry with IR stereo images +# for better pose estimation while seding RGB-D data to slam for +# a colored map. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + vo_parameters={ + 'frame_id':'camera_link', + 'wait_imu_to_init':True} + + vo_remappings=[ + ('imu', '/imu/data'), + ('left/image_rect', '/camera/infra1/image_rect_raw'), + ('left/camera_info', '/camera/infra1/camera_info'), + ('right/image_rect', '/camera/infra2/image_rect_raw'), + ('right/camera_info', '/camera/infra2/camera_info')] + + slam_parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False} + + slam_remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'align_depth.enable': 'true', + 'enable_sync': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[vo_parameters], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], + remappings=vo_remappings), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[slam_parameters], + remappings=slam_remappings, + arguments=['-d', LaunchConfiguration("args")]), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[slam_parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=slam_remappings), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')]), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index c5284b2c..a3e6c017 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -35,6 +35,14 @@ def generate_launch_description(): DeclareLaunchArgument( 'unite_imu_method', default_value='2', description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), @@ -56,13 +64,14 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=parameters, remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py new file mode 100644 index 00000000..92bb747e --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py @@ -0,0 +1,132 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_infra_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_infra.launch.py: the +# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# As in the non-composed example, the left infrared image (infra1) is used as +# the grayscale "RGB" input and paired with the depth stream. This works because +# on the D435i the depth is computed in the left-infrared frame, so infra1 and +# depth share the same intrinsics/frame (already registered, no align needed). +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/infra1/image_rect_raw'), + ('rgb/camera_info', '/camera/infra1/camera_info'), + ('depth/image', '/camera/depth/image_rect_raw')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': True, + 'enable_infra2': True, + 'enable_sync': True, + 'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # RGB-D odometry (infra1 as grayscale RGB + depth) + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index ccf865d3..9631f300 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -3,7 +3,21 @@ # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py - +# +# +# +# +# VINS-Fusion example: +# Add to your ros2 workspace the package https://github.com/zinuok/VINS-Fusion-ROS2 +# Apply this patch https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd +# -> Revert "#define USE_GPU" change if you want to build VINS-Fusion with GPU support. +# That may be counterintuitive, but we need to build VINS-Fusion first, then rebuild rtabmap with VINS-Fusion support. +# -> in your ros2 workspace, do "colcon build --packages-select vins" +# -> go back under rtabmap library repo, then rebuild/install with "cmake -DWITH_VINS_FUSION=ON ..." +# -> do "colcon build" in your ros2 workspace again to rebuild rtabmap_ros +# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py odom_args:="--Odom/Strategy 9 OdomVINSFusion/ConfigPath ~/ros2_ws/src/VINS-Fusion-ROS2/config/realsense_d435i/realsense_stereo_imu_config.yaml" +# -> set "imu: 1" in realsense_stereo_imu_config.yaml to do stereo inertial odometry, otherwise only stereo odometry is done. +# import os from ament_index_python.packages import get_package_share_directory @@ -16,11 +30,11 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration def generate_launch_description(): - parameters=[{ + parameters={ 'frame_id':'camera_link', 'subscribe_stereo':True, 'subscribe_odom_info':True, - 'wait_imu_to_init':True}] + 'wait_imu_to_init':True} remappings=[ ('imu', '/imu/data'), @@ -38,6 +52,15 @@ def generate_launch_description(): #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + # Launch camera driver IncludeLaunchDescription( @@ -55,18 +78,20 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', - parameters=parameters, + parameters=[parameters], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=parameters, + parameters=[parameters], remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=parameters, + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], remappings=remappings), # Compute quaternion of the IMU diff --git a/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py new file mode 100644 index 00000000..0417859f --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py @@ -0,0 +1,128 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_stereo_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_stereo.launch.py: the +# camera driver, IMU filter, stereo odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_stereo':True, + 'subscribe_odom_info':True, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('left/image_rect', '/camera/infra1/image_rect_raw'), + ('left/camera_info', '/camera/infra1/camera_info'), + ('right/image_rect', '/camera/infra2/image_rect_raw'), + ('right/camera_info', '/camera/infra2/camera_info')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': True, + 'enable_infra2': True, + 'enable_sync': True, + 'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # Stereo odometry + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/rtabmap_D405x2.launch.py b/rtabmap_examples/launch/rtabmap_D405x2.launch.py index 7a8fda54..b51e66f0 100644 --- a/rtabmap_examples/launch/rtabmap_D405x2.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x2.launch.py @@ -74,7 +74,6 @@ def generate_launch_description(): remappings=[ ("rgbd_image", '/realsense_camera1/rgbd_image'), ("odom", 'odom')], - arguments=["--delete_db_on_start", ''], prefix='', namespace='rtabmap' ) diff --git a/rtabmap_examples/launch/rtabmap_D405x3.launch.py b/rtabmap_examples/launch/rtabmap_D405x3.launch.py index f2d6f61f..5278cb9a 100644 --- a/rtabmap_examples/launch/rtabmap_D405x3.launch.py +++ b/rtabmap_examples/launch/rtabmap_D405x3.launch.py @@ -88,7 +88,6 @@ def generate_launch_description(): remappings=[ ("rgbd_image", '/realsense_camera1/rgbd_image'), ("odom", 'odom')], - arguments=["--delete_db_on_start", ''], prefix='', namespace='rtabmap' ) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index 80ce57a5..2b95d161 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -23,13 +23,26 @@ remappings = [] def launch_setup(context: LaunchContext, *args, **kwargs): - # Hack to override grab_resolution parameter without changing any files + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"] + + # Override some ZED parameters without changing any files: + # * grab_resolution: VGA + # * pos_tracking_enabled: disabled when rtabmap computes the odometry, so + # the ZED node does not publish the odom->camera_link TF (which would + # conflict with rtabmap's odometry). We still set publish_tf:=true below + # so the ZED node keeps broadcasting the IMU TF, which rtabmap needs + # (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf + # is false, and the IMU frame is not in the ZED URDF, so this is the only + # way to get the IMU TF while rtabmap owns the odometry. + pos_tracking_enabled = 'true' if use_zed_odometry else 'false' with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: zed_override_file.write("---\n"+ "/**:\n"+ " ros__parameters:\n"+ " general:\n"+ - " grab_resolution: 'VGA'") + " grab_resolution: 'VGA'\n"+ + " pos_tracking:\n"+ + " pos_tracking_enabled: "+pos_tracking_enabled) parameters=[{'frame_id':'zed_camera_link', 'subscribe_rgbd':True, @@ -38,7 +51,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): remappings=[('imu', '/zed/zed_node/imu/data')] - if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]: + if use_zed_odometry: remappings.append(('odom', '/zed/zed_node/odom')) else: parameters.append({'subscribe_odom_info': True}) @@ -51,7 +64,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): '/zed_camera.launch.py']), launch_arguments={'camera_model': LaunchConfiguration('camera_model'), 'ros_params_override_path': zed_override_file.name, - 'publish_tf': LaunchConfiguration('use_zed_odometry'), + 'publish_tf': 'true', + 'publish_imu_tf': 'true', 'publish_map_tf': 'false'}.items(), ), @@ -59,8 +73,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', parameters=parameters, - remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), - ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + remappings=[('rgb/image', '/zed/zed_node/rgb/color/rect/image'), + ('rgb/camera_info', '/zed/zed_node/rgb/color/rect/camera_info'), ('depth/image', '/zed/zed_node/depth/depth_registered')]), # Visual odometry diff --git a/rtabmap_examples/launch/zed_composition.launch.py b/rtabmap_examples/launch/zed_composition.launch.py new file mode 100644 index 00000000..2045a271 --- /dev/null +++ b/rtabmap_examples/launch/zed_composition.launch.py @@ -0,0 +1,164 @@ +# Requirements: +# A ZED camera +# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper) +# Example: +# $ ros2 launch rtabmap_examples zed_composition.launch.py camera_model:=zed2i +# +# This is the "composition" variant of zed.launch.py: the ZED driver, RGB-D +# synchronization, visual odometry and SLAM all run as composable nodes in a +# single component container with use_intra_process_comms enabled, so messages +# can be passed by pointer instead of being serialized/copied between processes. +# +# Notes: +# * The ZED wrapper's zed_camera.launch.py already creates its own component +# container ("zed_container") and loads the ZedCamera component into it with +# intra-process comms enabled by default (enable_ipc:=true). So instead of +# creating our own container, we let the ZED wrapper create it and load the +# rtabmap nodes into the SAME container (/zed/zed_container) with +# LoadComposableNodes. This assumes the default ZED namespace ("zed"), which +# is independent of camera_model. +# * ComposableNode has no "arguments" or "condition" field. So the '-d' +# argument becomes the 'delete_db_on_start' parameter, and the conditional +# odometry node is included in Python depending on use_zed_odometry. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def launch_setup(context: LaunchContext, *args, **kwargs): + + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"] + + # Override some ZED parameters without changing any files: + # * grab_resolution: VGA + # * pos_tracking_enabled: disabled when rtabmap computes the odometry, so + # the ZED node does not publish the odom->camera_link TF (which would + # conflict with rtabmap's odometry). We still set publish_tf:=true below + # so the ZED node keeps broadcasting the IMU TF, which rtabmap needs + # (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf + # is false, and the IMU frame is not in the ZED URDF, so this is the only + # way to get the IMU TF while rtabmap owns the odometry. + pos_tracking_enabled = 'true' if use_zed_odometry else 'false' + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'\n"+ + " pos_tracking:\n"+ + " pos_tracking_enabled: "+pos_tracking_enabled) + + # ZED topics are /zed/zed_node/* and the container created by the wrapper is + # /zed/zed_container (default ZED namespace "zed"). + zed_ns = '/zed/zed_node' + zed_container = '/zed/zed_container' + + parameters=[{'frame_id':'zed_camera_link', + 'subscribe_rgbd':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', zed_ns + '/imu/data')] + + if use_zed_odometry: + remappings.append(('odom', zed_ns + '/odom')) + else: + parameters.append({'subscribe_odom_info': True}) + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + # rtabmap nodes loaded into the ZED container. + composable_nodes = [ + # Sync rgb/depth/camera_info together + ComposableNode( + package='rtabmap_sync', plugin='rtabmap_sync::RGBDSync', + parameters=parameters, + remappings=[('rgb/image', zed_ns + '/rgb/color/rect/image'), + ('rgb/camera_info', zed_ns + '/rgb/color/rect/camera_info'), + ('depth/image', zed_ns + '/depth/depth_registered')], + extra_arguments=intra_process), + ] + + # Visual odometry (only when not using ZED's own odometry). ComposableNode + # has no 'condition', so we add it here based on use_zed_odometry. + if not use_zed_odometry: + composable_nodes.append( + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=parameters, + remappings=remappings, + extra_arguments=intra_process)) + + # VSLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph topics + # by default (transient_local QoS), which is incompatible with intra-process + # comms ("intraprocess communication allowed only with volatile durability"). + # latch=False makes them volatile so the node can join the container. + composable_nodes.append( + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=parameters + [{'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process)) + + return [ + # Launch camera driver. It creates the "zed_container" component + # container (enable_ipc:=true by default) and loads the ZedCamera + # component into it. + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + # publish_tf must be true so the ZED node broadcasts the + # IMU TF (gated by publish_tf). The odom->camera_link TF is + # disabled via pos_tracking_enabled=false (override file) + # when rtabmap computes the odometry. + 'publish_tf': 'true', + 'publish_imu_tf': 'true', + 'publish_map_tf': 'false'}.items(), + ), + + # Load the rtabmap pipeline into the ZED container. + LoadComposableNodes( + target_container=zed_container, + composable_node_descriptions=composable_nodes), + + # Visualization + # Note: rtabmap_viz is a Qt application; its UI must run in the process + # main thread, while components run in container worker threads. So it + # cannot be composed and stays a standalone node. + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ] + + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_zed_odometry', default_value='false', + description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 6cb5d2a0..ddf77bdd 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -2,7 +2,7 @@ rtabmap_examples - 0.22.1 + 0.23.7 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 26e4d549..416688ab 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -40,7 +40,17 @@ class ConditionalBool(Substitution): return self.text_else def launch_setup(context, *args, **kwargs): - + + rtabmap_viz_odometry_node_name = "rgbd_odometry" + use_icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + use_icp_odometry = use_icp_odometry == 'true' or use_icp_odometry == 'True' + use_stereo_odometry = LaunchConfiguration('stereo').perform(context) + use_stereo_odometry = use_stereo_odometry == 'true' or use_stereo_odometry == 'True' + if use_icp_odometry: + rtabmap_viz_odometry_node_name = "icp_odometry" + elif use_stereo_odometry: + rtabmap_viz_odometry_node_name = "stereo_odometry" + return [ DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''), DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''), @@ -53,6 +63,7 @@ def launch_setup(context, *args, **kwargs): DeclareLaunchArgument('qos_user_data', default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_imu', default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('qos_gps', default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'), + DeclareLaunchArgument('qos_env_sensor', default_value=LaunchConfiguration('qos'), description='Specific QoS used for env sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'), DeclareLaunchArgument('odom_log_level', default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'), @@ -186,7 +197,8 @@ def launch_setup(context, *args, **kwargs): "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), - "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'), + "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}], remappings=[ ("rgb/image", LaunchConfiguration('rgb_topic_relay')), ("depth/image", LaunchConfiguration('depth_topic_relay')), @@ -223,7 +235,8 @@ def launch_setup(context, *args, **kwargs): "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'), "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), - "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'), + "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}], remappings=[ ("left/image_rect", LaunchConfiguration('left_image_topic_relay')), ("right/image_rect", LaunchConfiguration('right_image_topic_relay')), @@ -258,7 +271,8 @@ def launch_setup(context, *args, **kwargs): "qos_imu": LaunchConfiguration('qos_imu'), "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context), "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'), - "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}], + "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'), + "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}], remappings=[ ("scan", LaunchConfiguration('scan_topic')), ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), @@ -303,6 +317,7 @@ def launch_setup(context, *args, **kwargs): "qos_camera_info": LaunchConfiguration('qos_camera_info'), "qos_imu": LaunchConfiguration('qos_imu'), "qos_gps": LaunchConfiguration('qos_gps'), + "qos_env_sensor": LaunchConfiguration('qos_env_sensor'), "qos_user_data": LaunchConfiguration('qos_user_data'), "scan_normal_k": LaunchConfiguration('scan_normal_k'), "landmark_linear_variance": LaunchConfiguration('tag_linear_variance'), @@ -327,6 +342,7 @@ def launch_setup(context, *args, **kwargs): ("gps/fix", LaunchConfiguration('gps_topic')), ("tag_detections", LaunchConfiguration('tag_topic')), ("fiducial_transforms", LaunchConfiguration('fiducial_topic')), + ("env_sensor", LaunchConfiguration('env_sensor_topic')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic')), ("goal_out", LaunchConfiguration('output_goal_topic'))], @@ -356,7 +372,8 @@ def launch_setup(context, *args, **kwargs): "qos_scan": LaunchConfiguration('qos_scan'), "qos_odom": LaunchConfiguration('qos_odom'), "qos_camera_info": LaunchConfiguration('qos_camera_info'), - "qos_user_data": LaunchConfiguration('qos_user_data') + "qos_user_data": LaunchConfiguration('qos_user_data'), + "odometry_node_name": rtabmap_viz_odometry_node_name }], remappings=[ ("rgb/image", LaunchConfiguration('rgb_topic_relay')), @@ -495,6 +512,7 @@ def generate_launch_description(): DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''), DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''), DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''), + DeclareLaunchArgument('odom_always_process_most_recent_frame', default_value='true', description='Odometry: always process latest frame to reduce delay, skipping frames in case odometry is slower than camera frame rate. In case you want to make sure to process all frames (e.g., from a rosbag/dataset) and you don\'t care about delay, set this to false.'), # imu DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'), @@ -514,6 +532,8 @@ def generate_launch_description(): DeclareLaunchArgument('tag_linear_variance', default_value='0.0001', description=''), DeclareLaunchArgument('tag_angular_variance', default_value='9999.0', description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'), DeclareLaunchArgument('fiducial_topic', default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'), + + DeclareLaunchArgument('env_sensor_topic', default_value='/env_sensor', description='A rtabmap_msgs/EnvSensor topic.'), OpaqueFunction(function=launch_setup) ]) diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 4f5f13dd..dba2b1f6 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -2,7 +2,7 @@ rtabmap_launch - 0.22.1 + 0.23.7 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/msg/EnvSensor.msg b/rtabmap_msgs/msg/EnvSensor.msg index 1a4ffe51..b3c40553 100644 --- a/rtabmap_msgs/msg/EnvSensor.msg +++ b/rtabmap_msgs/msg/EnvSensor.msg @@ -1,6 +1,27 @@ std_msgs/Header header -# EnvSensor +# Environmental sensor + +# built-in types +int32 TYPE_UNDEFINED=0 +int32 TYPE_WIFI_SIGNAL_STRENGTH=1 # dBm +int32 TYPE_AMBIENT_TEMPERATURE=2 # Celcius +int32 TYPE_AMBIENT_AIR_PRESSURE=3 # hPa +int32 TYPE_AMBIENT_LIGHT=4 # lx +int32 TYPE_AMBIENT_RELATIVE_HUMIDITY=5 # % + +# user types +int32 TYPE_CUSTOM1=100 +int32 TYPE_CUSTOM2=101 +int32 TYPE_CUSTOM3=102 +int32 TYPE_CUSTOM4=103 +int32 TYPE_CUSTOM5=104 +int32 TYPE_CUSTOM6=105 +int32 TYPE_CUSTOM7=106 +int32 TYPE_CUSTOM8=107 +int32 TYPE_CUSTOM9=108 + int32 type + float64 value \ No newline at end of file diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index c03cb92e..1a26dab8 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -2,7 +2,7 @@ rtabmap_msgs - 0.22.1 + 0.23.7 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index 96130502..7b10ad25 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/rtabmap_odom/CMakeLists.txt @@ -41,7 +41,25 @@ include_directories( ) SET(Libraries + cv_bridge::cv_bridge + rclcpp_components::component + image_geometry::image_geometry + laser_geometry::laser_geometry + message_filters::message_filters + pcl_conversions::pcl_conversions +) +SET(PublicLibraries + rclcpp::rclcpp + sensor_msgs::sensor_msgs + nav_msgs::nav_msgs + rtabmap_conversions::rtabmap_conversions + rtabmap_msgs::rtabmap_msgs + rtabmap_util::rtabmap_util + rtabmap_sync::rtabmap_sync +) +SET(AmentLibraries cv_bridge + rclcpp_components image_geometry laser_geometry message_filters @@ -56,6 +74,10 @@ SET(Libraries rtabmap_sync ) +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + add_definitions(-DPRE_ROS_LYRICAL) +ENDIF() + ########### ## Build ## ########### @@ -81,10 +103,13 @@ target_include_directories(rtabmap_odom ) add_library(rtabmap_odom_plugins SHARED ${rtabmap_odom_plugins_lib_src}) -ament_target_dependencies(rtabmap_odom ${Libraries}) -ament_target_dependencies(rtabmap_odom_plugins ${Libraries}) - -target_link_libraries(rtabmap_odom_plugins rtabmap_odom) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_odom ${AmentLibraries}) +else() + target_link_libraries(rtabmap_odom PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_odom_plugins PUBLIC ${Libraries}) +endif() +target_link_libraries(rtabmap_odom_plugins PUBLIC rtabmap_odom) rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::RGBDOdometry") rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::StereoOdometry") @@ -92,25 +117,21 @@ rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::ICPOdometry add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp) -ament_target_dependencies(rtabmap_rgbd_odometry ${Libraries}) -target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_rgbd_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry") add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp) -ament_target_dependencies(rtabmap_stereo_odometry ${Libraries}) -target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_stereo_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry") add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp) -ament_target_dependencies(rtabmap_icp_odometry ${Libraries}) -target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_icp_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_odom rtabmap_odom_plugins) # To include downstream without targets diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 4a1b7a81..ca0df5bc 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -30,9 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rclcpp/rclcpp.hpp" -#include -#include -#include +#include +#include +#include #include @@ -98,7 +98,7 @@ protected: virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {} private: - + void processData(); virtual void mainLoop(); virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} @@ -123,6 +123,8 @@ private: double guessMinTranslation_; double guessMinRotation_; double guessMinTime_; + double guessLinearVariance_; + double guessAngularVariance_; bool publishTf_; double waitForTransform_; bool publishNullWhenLost_; @@ -174,9 +176,12 @@ private: rtabmap::Transform guessPreviousPose_; double previousStamp_; double previousClockTime_; + double lastReceivedTopicClock_; + double lastReceivedTopicStamp_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; + bool alwaysProcessMostRecentFrame_; std::string compressionImgFormat_; bool compressionParallelized_; int odomStrategy_; diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp index 28ab0e55..dc50b450 100644 --- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp @@ -78,6 +78,7 @@ private: double scanNormalGroundUp_; bool deskewing_; bool deskewingSlerp_; + int topicQueueSize_; //std::vector > plugins_; //pluginlib::ClassLoader plugin_loader_; bool scanReceived_ = false; diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 9343fa49..52649daf 100644 --- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp @@ -151,6 +151,7 @@ private: int topicQueueSize_; int syncQueueSize_; bool keepColor_; + double approxSyncMaxInterval_; }; } diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index 1429bc85..5b47c8d0 100644 --- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp @@ -149,6 +149,7 @@ private: int topicQueueSize_; int syncQueueSize_; bool keepColor_; + double approxSyncMaxInterval_; }; } diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 245a8d4e..14870d32 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -2,7 +2,7 @@ rtabmap_odom - 0.22.1 + 0.23.7 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index c9955d99..bc34b042 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -76,6 +76,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o guessMinTranslation_(0.0), guessMinRotation_(0.0), guessMinTime_(0.0), + guessLinearVariance_(0.001), + guessAngularVariance_(0.001), publishTf_(true), waitForTransform_(0.1), // 100 ms publishNullWhenLost_(true), @@ -89,9 +91,12 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o icpParams_(false), previousStamp_(0.0), previousClockTime_(0.0), + lastReceivedTopicClock_(0.0), + lastReceivedTopicStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), + alwaysProcessMostRecentFrame_(true), compressionImgFormat_(".jpg"), compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), @@ -140,10 +145,13 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o guessMinTranslation_ = this->declare_parameter("guess_min_translation", guessMinTranslation_); guessMinRotation_ = this->declare_parameter("guess_min_rotation", guessMinRotation_); guessMinTime_ = this->declare_parameter("guess_min_time", guessMinTime_); + guessLinearVariance_ = this->declare_parameter("guess_linear_variance", guessLinearVariance_); + guessAngularVariance_ = this->declare_parameter("guess_angular_variance", guessAngularVariance_); expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_); maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_); minUpdateRate_ = this->declare_parameter("min_update_rate", minUpdateRate_); + alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_); compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_); compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); @@ -201,6 +209,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_translation = %f", guessMinTranslation_); RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_rotation = %f", guessMinRotation_); RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time = %f", guessMinTime_); + RCLCPP_INFO(this->get_logger(), "Odometry: guess_linear_variance = %f", guessLinearVariance_); + RCLCPP_INFO(this->get_logger(), "Odometry: guess_angular_variance = %f", guessAngularVariance_); RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate = %f Hz", expectedUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_); @@ -360,15 +370,16 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams) odometry_->reset(initialPose_); } - resetSrv_ = this->create_service("reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resetToPoseSrv_ = this->create_service("reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pauseSrv_ = this->create_service("pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service("resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + const std::string servicePrefix = get_name() + std::string("/"); + resetSrv_ = this->create_service(servicePrefix + "reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resetToPoseSrv_ = this->create_service(servicePrefix + "reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + pauseSrv_ = this->create_service(servicePrefix + "pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogDebugSrv_ = this->create_service("log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogInfoSrv_ = this->create_service("log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogWarnSrv_ = this->create_service("log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogErrorSrv_ = this->create_service("log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); odomStrategy_ = 0; Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); @@ -461,25 +472,47 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + double clockNow = rtabmap_conversions::timestampFromROS(now()); if(dataMutex_.lockTry() == 0) { if(bufferedDataToProcess_) { - RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.", rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp)); ++droppedMsgs_; } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; - dataReady_.release(); + if(alwaysProcessMostRecentFrame_) { + dataReady_.release(); + } dataMutex_.unlock(); ++processedMsgs_; + if(!alwaysProcessMostRecentFrame_) { + processData(); + } } else { - //RCLCPP_WARN(get_logger(), "Dropping image/scan data"); + double estimatedPeriod = clockNow - lastReceivedTopicClock_; + double topicPeriod = rtabmap_conversions::timestampFromROS(header.stamp) - lastReceivedTopicStamp_; + if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.5) { + RCLCPP_WARN(get_logger(), + "Dropping image/scan data with stamp %f (delay=%f). Something is wrong " + "because the clock difference with the previous topic received (%fs) is much lower than the " + "expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing " + "a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" " + "to avoid aggressively dropping data.", + rtabmap_conversions::timestampFromROS(header.stamp), + clockNow - rtabmap_conversions::timestampFromROS(header.stamp), + estimatedPeriod, + topicPeriod, + lastReceivedTopicStamp_); + } ++droppedMsgs_; } + lastReceivedTopicStamp_ = rtabmap_conversions::timestampFromROS(header.stamp); + lastReceivedTopicClock_ = clockNow; } void OdometryROS::mainLoopKill() @@ -497,7 +530,10 @@ void OdometryROS::mainLoop() // thread killed return; } - + processData(); +} +void OdometryROS::processData() +{ UScopeMutex lock(dataMutex_); // aliases @@ -516,21 +552,35 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) { - RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(imus_.empty()) { + // If empty, it is an error! + RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp()); + } bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + std::map::iterator iterLast = iterEnd; if(iterEnd!= imus_.end()) { ++iterEnd; } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + std::map::iterator iterFirst = imus_.begin(); + for(std::map::iterator iter=iterFirst; iter!=iterEnd;) { - imus.push_back(*iter); - imus_.erase(iter++); + // Because we always keep the last processed imu in the buffer, skip the first + // one when processing again the buffer + if(iter!=iterFirst) { + imus.push_back(*iter); + } + if(iter!=iterLast) { + imus_.erase(iter++); + } + else { + ++iter; + } } } // end imu lock @@ -666,6 +716,44 @@ void OdometryROS::mainLoop() } } + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_; + if(tooOldPreviousData) + { + RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update " + "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", + rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); + + if(!guess_.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!", + guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str()); + odometry_->reset(odometry_->getPose() * guess_); + guess_.setNull(); + guessPreviousPose_.setNull(); + } + else + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_); + if(tfPose.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + } + } + + bool skipOdometryUpdate = false; + + rtabmap::Transform pose; + rtabmap::OdometryInfo info; + rtabmap::Transform guessVelocity; Transform guessCurrentPose; if(!guessFrameId_.empty()) @@ -702,28 +790,22 @@ void OdometryROS::mainLoop() (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ < guessMinTime_))) { // Ignore odometry update, we didn't move enough - if(publishTf_) - { - geometry_msgs::msg::TransformStamped correctionMsg; - correctionMsg.child_frame_id = guessFrameId_; - correctionMsg.header.frame_id = odomFrameId_; - correctionMsg.header.stamp = header.stamp; - Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); - rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); + pose = odometry_->getPose() * guess_; + info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1); + info.reg.covariance.at(0,0) = guessLinearVariance_; // xx + info.reg.covariance.at(1,1) = guessLinearVariance_; // yy + info.reg.covariance.at(2,2) = guessLinearVariance_; // zz + info.reg.covariance.at(3,3) = guessAngularVariance_; // rr + info.reg.covariance.at(4,4) = guessAngularVariance_; // pp + info.reg.covariance.at(5,5) = guessAngularVariance_; // yawyaw - double time_now = now().seconds(); - if(time_now >= previousClockTime_) { - tfBroadcaster_->sendTransform(correctionMsg); - } - else { - RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.", - correctionMsg.header.frame_id.c_str(), - correctionMsg.child_frame_id.c_str(), - previousClockTime_ - time_now); - } - } - guessPreviousPose_ = guessCurrentPose; - return; + //set velocity + double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_; + UASSERT(dt>0.0); + // use part of guess matching dt + (previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt); + skipOdometryUpdate = true; } } guessPreviousPose_ = guessCurrentPose; @@ -735,23 +817,21 @@ void OdometryROS::mainLoop() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_; - // process data rclcpp::Time timeStart = rclcpp::Clock().now(); - rtabmap::OdometryInfo info; if(!groundTruth.isNull()) { data.setGroundTruth(groundTruth); } - rtabmap::Transform pose; - if(!tooOldPreviousData) + if(!skipOdometryUpdate) { pose = odometry_->process(data, guess_, &info); } if(!pose.isNull()) { - guess_.setNull(); + if(!skipOdometryUpdate) { + guess_.setNull(); + } resetCurrentCount_ = resetCountdown_; //********************* @@ -825,11 +905,16 @@ void OdometryROS::mainLoop() odom.pose.covariance.at(35) = info.reg.covariance.at(5,5)*2; // yawyaw //set velocity - bool setTwist = !odometry_->getVelocityGuess().isNull(); + bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull(); if(setTwist) { float x,y,z,roll,pitch,yaw; - odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + if(skipOdometryUpdate) { + UASSERT(!guessVelocity.isNull()); + guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } else { + odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } odom.twist.twist.linear.x = x; odom.twist.twist.linear.y = y; odom.twist.twist.linear.z = z; @@ -874,7 +959,7 @@ void OdometryROS::mainLoop() odomLocalMap_->publish(cloudMsg); } - if(odomLastFrame_->get_subscription_count()) + if(!skipOdometryUpdate && odomLastFrame_->get_subscription_count()) { // check which type of Odometry is using if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry @@ -1005,20 +1090,14 @@ void OdometryROS::mainLoop() } - if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData)) + if(pose.isNull() && resetCurrentCount_ > 0) { - if(tooOldPreviousData) - { - RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update " - "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); - } - else if(--resetCurrentCount_>0) + if(--resetCurrentCount_>0) { RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); } - if(resetCurrentCount_ == 0 || tooOldPreviousData) + if(resetCurrentCount_ == 0) { if(!guess_.isNull()) { @@ -1181,9 +1260,11 @@ void OdometryROS::mainLoop() msg.header.stamp = header.stamp; // use corresponding time stamp to image odomSensorDataCompressedPub_->publish(msg); } - double delay = (now()-header.stamp).seconds(); - if(visParams_) + if(skipOdometryUpdate) { + RCLCPP_INFO(this->get_logger(), "Odom: , std dev=%fm|%frad, update time=%fs, delay=%fs", 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(visParams_) { if(icpParams_) { @@ -1241,6 +1322,8 @@ void OdometryROS::reset(const Transform & pose) guessPreviousPose_.setNull(); previousStamp_ = 0.0; previousClockTime_ = 0.0; + lastReceivedTopicClock_ = 0.0; + lastReceivedTopicStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index d30b68aa..990976ec 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -60,6 +60,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) : scanNormalGroundUp_(0.0), deskewing_(false), deskewingSlerp_(false), + topicQueueSize_(1), scanReceived_(false), cloudReceived_(false) { @@ -83,6 +84,7 @@ void ICPOdometry::onOdomInit() scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_); deskewing_ = this->declare_parameter("deskewing", deskewing_); deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_); + topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos = %d", (int)qos()); RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); @@ -96,12 +98,13 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "IcpOdometry: topic_queue_size = %d", topicQueueSize_); rclcpp::SubscriptionOptions options; options.callback_group = dataCallbackGroup_; - scan_sub_ = create_subscription("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); + scan_sub_ = create_subscription("scan", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 9976065f..97d48f6e 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -65,7 +65,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : exactSync6_(0), topicQueueSize_(10), syncQueueSize_(5), - keepColor_(false) + keepColor_(false), + approxSyncMaxInterval_(0.0) { OdometryROS::init(false, true, false); } @@ -91,9 +92,8 @@ void RGBDOdometry::onOdomInit() int rgbdCameras = 1; bool approxSync = true; bool subscribeRGBD = false; - double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); - approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); + approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_); topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); int queueSize = this->declare_parameter("queue_size", -1); if(queueSize != -1) @@ -113,12 +113,20 @@ void RGBDOdometry::onOdomInit() rgbdCameras = 0; } keepColor_ = this->declare_parameter("keep_color", keepColor_); - std::string rgbdTransport = this->declare_parameter("rgb_transport", std::string("raw")); + std::string rgbTransport = this->declare_parameter("rgb_transport", std::string("raw")); + if(rgbTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"rgb_transport\" has been renamed " + "to \"image_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"image_transport\".", rgbTransport.c_str()); + } + std::string imageTransport = this->declare_parameter("image_transport", rgbTransport); std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw")); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos()); @@ -126,7 +134,7 @@ void RGBDOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgb_transport = %s", rgbdTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: image_transport = %s", imageTransport.c_str()); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str()); rclcpp::SubscriptionOptions options; @@ -165,8 +173,8 @@ void RGBDOdometry::onOdomInit() MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); } else @@ -180,7 +188,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getSubscriber()->get_topic_name(), rgbd_image2_sub_.getSubscriber()->get_topic_name()); } @@ -193,8 +201,8 @@ void RGBDOdometry::onOdomInit() rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else @@ -209,7 +217,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getSubscriber()->get_topic_name(), rgbd_image2_sub_.getSubscriber()->get_topic_name(), rgbd_image3_sub_.getSubscriber()->get_topic_name()); @@ -224,8 +232,8 @@ void RGBDOdometry::onOdomInit() rgbd_image2_sub_, rgbd_image3_sub_, rgbd_image4_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } else @@ -241,7 +249,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getSubscriber()->get_topic_name(), rgbd_image2_sub_.getSubscriber()->get_topic_name(), rgbd_image3_sub_.getSubscriber()->get_topic_name(), @@ -258,8 +266,8 @@ void RGBDOdometry::onOdomInit() rgbd_image3_sub_, rgbd_image4_sub_, rgbd_image5_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); } else @@ -276,7 +284,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getSubscriber()->get_topic_name(), rgbd_image2_sub_.getSubscriber()->get_topic_name(), rgbd_image3_sub_.getSubscriber()->get_topic_name(), @@ -295,8 +303,8 @@ void RGBDOdometry::onOdomInit() rgbd_image4_sub_, rgbd_image5_sub_, rgbd_image6_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); } else @@ -314,7 +322,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -354,25 +362,26 @@ void RGBDOdometry::onOdomInit() } else { - image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport"); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints rgb_hints(this); // using "image_transport" parameter image_transport::TransportHints depth_hints(this, "raw", "depth_transport"); - - std::string rgb_topic = get_node_base_interface()->resolve_topic_or_service_name( - "rgb/image", false, false - ); - std::string depth_topic = get_node_base_interface()->resolve_topic_or_service_name( - "depth/image", false, false - ); - - image_mono_sub_.subscribe(this, rgb_topic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); - image_depth_sub_.subscribe(this, depth_topic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_mono_sub_.subscribe(this, rgbTopic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_depth_sub_.subscribe(this, depthTopic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); +#else + image_transport::TransportHints rgb_hints(*this); // using "image_transport" parameter + image_transport::TransportHints depth_hints(*this, "raw", "depth_transport"); + image_mono_sub_.subscribe(*this, rgbTopic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options); + image_depth_sub_.subscribe(*this, depthTopic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options); +#endif info_sub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else @@ -385,7 +394,7 @@ void RGBDOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", topicQueueSize_, syncQueueSize_, image_mono_sub_.getSubscriber().getTopic().c_str(), @@ -430,8 +439,10 @@ void RGBDOdometry::commonCallback( { UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); rclcpp::Time higherStamp; + UASSERT_MSG(rgbImages[0], "RGB image is null!"); int imageWidth = rgbImages[0]->image.cols; int imageHeight = rgbImages[0]->image.rows; + UASSERT_MSG(depthImages[0], "Depth image is null!"); int depthWidth = depthImages[0]->image.cols; int depthHeight = depthImages[0]->image.rows; @@ -445,6 +456,8 @@ void RGBDOdometry::commonCallback( std::vector cameraModels; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || @@ -589,12 +602,17 @@ void RGBDOdometry::callback( std::vector imageMsgs(1); std::vector depthMsgs(1); std::vector infoMsgs; - imageMsgs[0] = cv_bridge::toCvShare(image); - depthMsgs[0] = cv_bridge::toCvShare(depth); + try{ + imageMsgs[0] = cv_bridge::toCvShare(image); + depthMsgs[0] = cv_bridge::toCvShare(depth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } infoMsgs.push_back(*cameraInfo); double stampDiff = fabs(rtabmap_conversions::timestampFromROS(image->header.stamp) - rtabmap_conversions::timestampFromROS(depth->header.stamp)); - if(stampDiff > 0.020) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.020) { RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 00f1f9e2..f56b2cc3 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -65,7 +65,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : exactSync6_(0), topicQueueSize_(10), syncQueueSize_(5), - keepColor_(false) + keepColor_(false), + approxSyncMaxInterval_(0.0) { OdometryROS::init(true, true, false); } @@ -90,10 +91,9 @@ void StereoOdometry::onOdomInit() { bool approxSync = false; bool subscribeRGBD = false; - double approxSyncMaxInterval = 0.0; int rgbdCameras = 1; approxSync = this->declare_parameter("approx_sync", approxSync); - approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); + approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_); topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); int queueSize = this->declare_parameter("queue_size", -1); if(queueSize != -1) @@ -109,16 +109,18 @@ void StereoOdometry::onOdomInit() subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); keepColor_ = this->declare_parameter("keep_color", keepColor_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); 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: approx_sync_max_interval = %f", approxSyncMaxInterval_); 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_INFO(this->get_logger(), "StereoOdometry: image_transport = %s", imageTransport.c_str()); rclcpp::SubscriptionOptions options; options.callback_group = dataCallbackGroup_; @@ -156,8 +158,8 @@ void StereoOdometry::onOdomInit() MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2)); } else @@ -171,7 +173,7 @@ void StereoOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str()); } @@ -184,8 +186,8 @@ void StereoOdometry::onOdomInit() rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else @@ -200,7 +202,7 @@ void StereoOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str()); @@ -215,8 +217,8 @@ void StereoOdometry::onOdomInit() rgbd_image2_sub_, rgbd_image3_sub_, rgbd_image4_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } else @@ -232,7 +234,7 @@ void StereoOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -249,8 +251,8 @@ void StereoOdometry::onOdomInit() rgbd_image3_sub_, rgbd_image4_sub_, rgbd_image5_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); } else @@ -267,7 +269,7 @@ void StereoOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -286,8 +288,8 @@ void StereoOdometry::onOdomInit() rgbd_image4_sub_, rgbd_image5_sub_, rgbd_image6_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); } else @@ -305,7 +307,7 @@ void StereoOdometry::onOdomInit() subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -347,17 +349,25 @@ void StereoOdometry::onOdomInit() } else { - image_transport::TransportHints hints(this); - imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); - imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(this); // using "image_transport" parameter + imageRectLeft_.subscribe(this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); +#else + image_transport::TransportHints hints(*this); // using "image_transport" parameter + imageRectLeft_.subscribe(*this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options); + imageRectRight_.subscribe(*this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options); +#endif cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - if(approxSyncMaxInterval>0.0) - approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_>0.0) + approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } else @@ -370,11 +380,11 @@ void StereoOdometry::onOdomInit() 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():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", topicQueueSize_, syncQueueSize_, - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), + imageRectLeft_.getSubscriber().getTopic().c_str(), + imageRectRight_.getSubscriber().getTopic().c_str(), cameraInfoLeft_.getSubscriber()->get_topic_name(), cameraInfoRight_.getSubscriber()->get_topic_name()); } @@ -495,49 +505,6 @@ void StereoOdometry::commonCallback( return; } else - { - stereoTransform = rtabmap_conversions::getTransform( - rightCameraInfos[i].header.frame_id, - leftCameraInfos[i].header.frame_id, - leftCameraInfos[i].header.stamp, - tfBuffer(), - waitForTransform()); - if(stereoTransform.isNull()) - { - RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)", - Parameters::kRtabmapImagesAlreadyRectified().c_str(), - rightCameraInfos[i].header.frame_id.c_str(), - leftCameraInfos[i].header.frame_id.c_str()); - return; - } - else if(stereoTransform.isIdentity()) - { - RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! " - "Identity transform returned between left and right cameras. Verify that if TF between " - "the cameras is valid: \"rosrun tf tf_echo %s %s\".", - Parameters::kRtabmapImagesAlreadyRectified().c_str(), - rightCameraInfos[i].header.frame_id.c_str(), - leftCameraInfos[i].header.frame_id.c_str()); - return; - } - } - } - - rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform); - - if( stereoModel.baseline() == 0 && - alreadyRectified && - !rightCameraInfos[i].header.frame_id.empty() && - !leftCameraInfos[i].header.frame_id.empty()) - { - stereoTransform = rtabmap_conversions::getTransform( - leftCameraInfos[i].header.frame_id, - rightCameraInfos[i].header.frame_id, - leftCameraInfos[i].header.stamp, - tfBuffer(), - waitForTransform()); - - if(!stereoTransform.isNull() && stereoTransform.x()>0) { static bool warned = false; if(!warned) @@ -571,7 +538,7 @@ void StereoOdometry::commonCallback( { RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! " "Identity transform returned between left and right cameras. Verify that if TF between " - "the cameras is valid: \"rosrun tf tf_echo %s %s\".", + "the cameras is valid: \"ros2 run tf2_ros tf_echo %s %s\".", Parameters::kRtabmapImagesAlreadyRectified().c_str(), rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str()); @@ -722,13 +689,18 @@ void StereoOdometry::callback( std::vector rightMsgs(1); std::vector leftInfoMsgs; std::vector rightInfoMsgs; - leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft); - rightMsgs[0] = cv_bridge::toCvShare(imageRectRight); + try{ + leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft); + rightMsgs[0] = cv_bridge::toCvShare(imageRectRight); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } leftInfoMsgs.push_back(*cameraInfoLeft); rightInfoMsgs.push_back(*cameraInfoRight); double stampDiff = fabs(rtabmap_conversions::timestampFromROS(imageRectLeft->header.stamp) - rtabmap_conversions::timestampFromROS(imageRectRight->header.stamp)); - if(stampDiff > 0.010) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010) { RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is " "high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware " diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 67b36a40..4a5d7e84 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -2,7 +2,7 @@ rtabmap_python - 0.22.1 + 0.23.7 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/rtabmap_python/__init__.py b/rtabmap_python/rtabmap_python/__init__.py index 139597f9..8b2e94fe 100644 --- a/rtabmap_python/rtabmap_python/__init__.py +++ b/rtabmap_python/rtabmap_python/__init__.py @@ -1,2 +1,27 @@ - - +# Copyright 2025 matlabbe +# +# 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 matlabbe 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. diff --git a/rtabmap_python/rtabmap_python/compression.py b/rtabmap_python/rtabmap_python/compression.py deleted file mode 100644 index 140f7f6f..00000000 --- a/rtabmap_python/rtabmap_python/compression.py +++ /dev/null @@ -1,34 +0,0 @@ - -import zlib -import struct -import numpy as np - -def compress(data): - assert data.ndim == 1 or data.ndim == 2 - - dim1 = 1 - if data.ndim == 1: - dim1 = 1 - dim2 = len(data) - else: - dim1 = data.shape[0] - dim2 = data.shape[1] - - numpy_type_to_cvtype = {'uint8': 0, 'int8': 1, 'uint16': 2, - 'int16': 3, 'int32': 4, 'float32': 5, - 'float64': 6} - - compressed_data = bytearray(zlib.compress(data.tobytes())) - compressed_data.extend(struct.pack("iii", dim1, dim2, numpy_type_to_cvtype[data.dtype.name])) - - return compressed_data - -def uncompress(bytes): - cvtype_to_numpy_type = {0: 'uint8', 1: 'int8', 2: 'uint16', - 3: 'int16', 4: 'int32', 5: 'float32', - 6: 'float64'} - out = zlib.decompress(bytes[:len(bytes)-3*4]) - rows, cols, datatype = struct.unpack_from("iii", bytes, offset=len(bytes)-3*4) - data = np.frombuffer(out, dtype=cvtype_to_numpy_type[datatype]) - return data.reshape((rows, cols)) - diff --git a/rtabmap_python/rtabmap_python/cv_compression.py b/rtabmap_python/rtabmap_python/cv_compression.py new file mode 100644 index 00000000..0b8e1604 --- /dev/null +++ b/rtabmap_python/rtabmap_python/cv_compression.py @@ -0,0 +1,78 @@ +# Copyright 2025 matlabbe +# +# 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 matlabbe 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. + + +import struct +import zlib + +import numpy as np + + +def compress(data): + assert data.ndim == 1 or data.ndim == 2 + + dim1 = 1 + if data.ndim == 1: + dim1 = 1 + dim2 = len(data) + else: + dim1 = data.shape[0] + dim2 = data.shape[1] + + numpy_type_to_cvtype = { + 'uint8': 0, + 'int8': 1, + 'uint16': 2, + 'int16': 3, + 'int32': 4, + 'float32': 5, + 'float64': 6, + } + + compressed_data = bytearray(zlib.compress(data.tobytes())) + compressed_data.extend( + struct.pack('iii', dim1, dim2, numpy_type_to_cvtype[data.dtype.name]) + ) + + return compressed_data + + +def uncompress(data): + cvtype_to_numpy_type = { + 0: 'uint8', + 1: 'int8', + 2: 'uint16', + 3: 'int16', + 4: 'int32', + 5: 'float32', + 6: 'float64', + } + out = zlib.decompress(data[: len(data) - 3 * 4]) + rows, cols, datatype = struct.unpack_from('iii', data, offset=len(data) - 3 * 4) + data = np.frombuffer(out, dtype=cvtype_to_numpy_type[datatype]) + return data.reshape((rows, cols)) diff --git a/rtabmap_python/setup.py b/rtabmap_python/setup.py index a6e87e0f..720479d0 100644 --- a/rtabmap_python/setup.py +++ b/rtabmap_python/setup.py @@ -15,11 +15,13 @@ setup( zip_safe=True, maintainer='Mathieu Labbe', maintainer_email='matlabbe@gmail.com', - description='RTAB-Map\'s python package.', + description="RTAB-Map's python package.", license='BSD', - tests_require=['pytest'], + extras_require={ + 'test': ['pytest'], + }, entry_points={ 'console_scripts': [ ], }, -) \ No newline at end of file +) diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 7088e8b7..457a8d3a 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -2,7 +2,7 @@ rtabmap_ros - 0.22.1 + 0.23.7 RTAB-Map Stack @@ -26,6 +26,7 @@ rtabmap_sync rtabmap_util rtabmap_viz + ament_cmake diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index c1808ebe..4e47a684 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -14,6 +14,9 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") ) endif() +# Make sure it is first to prevent Qt6 from claiming the generic versionless targets first +find_package(Qt5 QUIET COMPONENTS Widgets) + find_package(ament_cmake_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) @@ -32,6 +35,19 @@ include_directories( ) SET(Libraries + pcl_conversions::pcl_conversions + pluginlib::pluginlib + rclcpp::rclcpp + rviz_common::rviz_common + rviz_rendering::rviz_rendering + rviz_default_plugins::rviz_default_plugins + sensor_msgs::sensor_msgs + std_msgs::std_msgs + tf2::tf2 + rtabmap_conversions::rtabmap_conversions + rtabmap_msgs::rtabmap_msgs +) +SET(AmentLibraries pcl_conversions pluginlib rclcpp @@ -45,9 +61,6 @@ SET(Libraries rtabmap_msgs ) -MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}") - - ########### ## Build ## ########### @@ -75,8 +88,11 @@ target_include_directories(rtabmap_rviz_plugins $ $ ) - -ament_target_dependencies(rtabmap_rviz_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_rviz_plugins ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_rviz_plugins PRIVATE ${Libraries}) +ENDIF() # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 326ab8f2..b5b67c82 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -2,7 +2,7 @@ rtabmap_rviz_plugins - 0.22.1 + 0.23.7 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe @@ -13,6 +13,7 @@ ament_cmake_ros ros_environment + qtbase5-private-dev pcl_conversions pluginlib diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index f53b7aaa..6107b23a 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -54,6 +54,22 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + geometry_msgs::geometry_msgs + nav_msgs::nav_msgs + rclcpp::rclcpp + rclcpp_components::component + sensor_msgs::sensor_msgs + std_msgs::std_msgs + std_srvs::std_srvs + tf2::tf2 + tf2_ros::tf2_ros + visualization_msgs::visualization_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_util::rtabmap_util + rtabmap_sync::rtabmap_sync +) +SET(AmentLibraries cv_bridge geometry_msgs nav_msgs @@ -74,6 +90,10 @@ if("$ENV{ROS_DISTRO}" STRLESS "jazzy") add_definitions(-DPRE_ROS_JAZZY) endif() +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + add_definitions(-DPRE_ROS_LYRICAL) +ENDIF() + ########### ## Build ## ########### @@ -88,6 +108,10 @@ MESSAGE(STATUS "WITH apriltag_msgs") ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS") SET(Libraries ${Libraries} + apriltag_msgs::apriltag_msgs +) +SET(AmentLibraries + ${AmentLibraries} apriltag_msgs ) ENDIF(apriltag_msgs_FOUND) @@ -98,6 +122,10 @@ MESSAGE(STATUS "WITH aruco_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_MSGS") SET(Libraries ${Libraries} + aruco_msgs::aruco_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_msgs ) ENDIF(aruco_msgs_FOUND) @@ -108,6 +136,10 @@ MESSAGE(STATUS "WITH aruco_opencv_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS") SET(Libraries ${Libraries} + aruco_opencv_msgs::aruco_opencv_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_opencv_msgs ) ENDIF(aruco_opencv_msgs_FOUND) @@ -118,6 +150,10 @@ MESSAGE(STATUS "WITH aruco_markers_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS") SET(Libraries ${Libraries} + aruco_markers_msgs::aruco_markers_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_markers_msgs ) ENDIF(aruco_markers_msgs_FOUND) @@ -128,6 +164,10 @@ MESSAGE(STATUS "WITH ros2_aruco_interfaces") ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES") SET(Libraries ${Libraries} + ros2_aruco_interfaces::ros2_aruco_interfaces +) +SET(AmentLibraries + ${AmentLibraries} ros2_aruco_interfaces ) ENDIF(ros2_aruco_interfaces_FOUND) @@ -138,6 +178,10 @@ MESSAGE(STATUS "WITH nav2_msgs") ADD_DEFINITIONS("-DWITH_NAV2_MSGS") SET(Libraries ${Libraries} + nav2_msgs::nav2_msgs +) +SET(AmentLibraries + ${AmentLibraries} nav2_msgs ) IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0) @@ -157,20 +201,23 @@ target_include_directories(rtabmap_slam_plugins $ ) -ament_target_dependencies(rtabmap_slam_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_slam_plugins ${AmentLibraries}) +else() + target_link_libraries(rtabmap_slam_plugins PUBLIC ${Libraries}) +endif() rclcpp_components_register_nodes(rtabmap_slam_plugins "rtabmap_slam::CoreWrapper") add_executable(rtabmap_node src/CoreNode.cpp) -ament_target_dependencies(rtabmap_node ${Libraries}) -target_link_libraries(rtabmap_node rtabmap_slam_plugins) +target_link_libraries(rtabmap_node PRIVATE rtabmap_slam_plugins) set_target_properties(rtabmap_node PROPERTIES OUTPUT_NAME "rtabmap") ############# ## Install ## ############# -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_slam_plugins) # To include downstream without targets diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 157a9b26..ca41388c 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -33,9 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include +#include +#include +#include #include #include @@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_msgs/msg/info.hpp" #include "rtabmap_msgs/msg/landmark_detection.hpp" #include "rtabmap_msgs/msg/landmark_detections.hpp" +#include "rtabmap_msgs/msg/env_sensor.h" #include "rtabmap_msgs/srv/get_nodes_in_radius.hpp" #include "rtabmap_msgs/srv/load_database.hpp" #include "rtabmap_msgs/srv/detect_more_loop_closures.hpp" @@ -191,6 +192,7 @@ private: void userDataAsyncCallback(const rtabmap_msgs::msg::UserData::SharedPtr dataMsg); void globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr globalPoseMsg); void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg); + void envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg); void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection); void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections); #ifdef WITH_APRILTAG_MSGS @@ -243,6 +245,8 @@ private: std::map filterNodesToAssemble( const std::map & nodes, const rtabmap::Transform & currentPose); + + void applyParameters(); void updateRtabmapCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); void resetRtabmapCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); @@ -334,6 +338,7 @@ private: double landmarkDefaultAngVariance_; double landmarkDefaultLinVariance_; double waitForTransform_; + double stalenessFactor_; bool useActionForGoal_; bool useSavedMap_; bool genScan_; @@ -444,6 +449,11 @@ private: std::map gps_; UMutex gpsMutex_; + rclcpp::CallbackGroup::SharedPtr envSensorAsyncCallbackGroup_; + rclcpp::Subscription::SharedPtr envSensorAsyncSub_; + rtabmap::EnvSensors envSensors_; + UMutex envSensorMutex_; + rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index c474cf18..99f37844 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -2,7 +2,7 @@ rtabmap_slam - 0.22.1 + 0.23.7 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 29b7e0e6..c1fb75b8 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -121,6 +121,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDefaultAngVariance_(0.001), landmarkDefaultLinVariance_(0.001), waitForTransform_(0.2),// 200 ms + stalenessFactor_(0.0), useActionForGoal_(false), useSavedMap_(true), genScan_(false), @@ -207,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_); waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); + stalenessFactor_ = this->declare_parameter("staleness_factor", stalenessFactor_); initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr); useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_); #ifndef WITH_NAV2_MSGS @@ -231,26 +233,26 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); - RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = %s", frameId_.c_str()); - if(!odomFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); - } - if(!groundTruthFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s", - groundTruthFrameId_.c_str(), - groundTruthBaseFrameId_.c_str()); - } - RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = %s", mapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = \"%s\" -> ground_truth_base_frame_id = \"%s\"", + groundTruthFrameId_.c_str(), + groundTruthBaseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = \"%s\"", mapFrameId_.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: log_to_rosout_level = %d", eventLevel); - RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = %s", initialPoseStr.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = \"%s\"", initialPoseStr.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay); 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(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) { + RCLCPP_ERROR(this->get_logger(), "rtabmap: staleness_factor should be 0 (disabled) or >= 1 (value that multiplies the detection update period). Current value is %f, setting it to 0...", + stalenessFactor_); + stalenessFactor_ = 0.0; + } + RCLCPP_INFO(this->get_logger(), "rtabmap: staleness_factor = %f", stalenessFactor_); if(this->isSubscribedToStereo()) { RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); @@ -768,9 +770,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } - image_transport::TransportHints hints(this); - 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); - + std::string imageTopic = this->get_node_topics_interface()->resolve_topic_name("image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(this); // using "image_transport" parameter + defaultSub_ = image_transport::create_subscription(this, imageTopic, 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); +#else + image_transport::TransportHints hints(*this); // using "image_transport" parameter + defaultSub_ = image_transport::create_subscription(*this, imageTopic, std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_), subOptions); +#endif RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); } @@ -831,6 +838,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rtabmap_.parseParameters(parameters_); } } + + if(!this->isSubscribedToOdom() && odomFrameId_.empty()) + { + bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); + if(isRGBD) { + RCLCPP_ERROR(this->get_logger(), "\"subscribe_odom\" or \"odom_frame_id\" should be used when \"%s\" is enabled!", Parameters::kRGBDEnabled().c_str()); + } + } // Set initial pose if set if(!initialPoseStr.empty()) @@ -861,24 +876,30 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + envSensorAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::SubscriptionOptions userDataAsyncSubOptions; rclcpp::SubscriptionOptions globalPoseAsyncSubOptions; rclcpp::SubscriptionOptions gpsAsyncSubOptions; rclcpp::SubscriptionOptions landmarkSubOptions; rclcpp::SubscriptionOptions imuSubOptions; + rclcpp::SubscriptionOptions envSensorAsyncSubOptions; userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_; globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_; gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_; landmarkSubOptions.callback_group = imuCallbackGroup_; imuSubOptions.callback_group = imuCallbackGroup_; + envSensorAsyncSubOptions.callback_group = envSensorAsyncCallbackGroup_; int qosGPS = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; int qosIMU = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + int qosEnvSensor = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); + qosEnvSensor = this->declare_parameter("qos_env_sensor", qosEnvSensor); 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); + envSensorAsyncSub_ = this->create_subscription("env_sensor", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosEnvSensor), std::bind(&CoreWrapper::envSensorAsyncCallback, this, std::placeholders::_1), envSensorAsyncSubOptions); 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 @@ -947,17 +968,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : parameters_.at(key) = vStr; } } - RCLCPP_INFO(this->get_logger(), "rtabmap: Updating parameters"); - if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) - { - rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate())); - RCLCPP_INFO(this->get_logger(), "RTAB-Map rate detection = %f Hz", rate_); - } - rtabmap_.parseParameters(parameters_); - // Don't reset map in localization mode - if(rtabmap_.getMemory()->isIncremental()) { - mapsManager_.setParameters(parameters_); - } + applyParameters(); } }; @@ -982,19 +993,28 @@ CoreWrapper::~CoreWrapper() this->saveParameters(configPath_); printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } - rtabmap_.close(); + rtabmap_.close(saveDatabase); printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); delete interOdomSync_; @@ -1137,6 +1157,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0])); triggerNewMapBeforeNextUpdate_ = true; lastPoseCovariance_ = cv::Mat(); + } + else if(stalenessFactor_>0.0 && + previousStamp_.seconds() > 0.0 && + rate_>0.0f && + (stamp - previousStamp_).seconds() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + (stamp - previousStamp_).seconds(), + stamp.seconds(), + previousStamp_.seconds(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1228,6 +1268,26 @@ bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Ti triggerNewMapBeforeNextUpdate_ = true; lastPoseCovariance_ = cv::Mat(); } + else if(stalenessFactor_>0.0 && + previousStamp_.seconds() > 0.0 && + rate_>0.0f && + (stamp - previousStamp_).seconds() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + (stamp - previousStamp_).seconds(), + stamp.seconds(), + previousStamp_.seconds(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); + } lastPoseIntermediate_ = false; lastPose_ = odom; @@ -1307,6 +1367,11 @@ void CoreWrapper::commonMultiCameraCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1693,6 +1758,11 @@ void CoreWrapper::commonLaserScanCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1902,6 +1972,11 @@ void CoreWrapper::commonSensorDataCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) { return; @@ -2190,6 +2265,16 @@ void CoreWrapper::process( data.setLandmarks(landmarks); } + // Env sensors + { + UScopeMutex lock(envSensorMutex_); + if(!envSensors_.empty()) + { + data.setEnvSensors(envSensors_); + envSensors_.clear(); + } + } + // IMU imuMutex_.lock(); if(!imus_.empty()) @@ -2350,8 +2435,8 @@ void CoreWrapper::process( if(rtabmap_.getMemory() == 0 || filteredPoses.size() == 0 || rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || - rtabmap_.getMemory()->getLastWorkingSignature() == 0 || - rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(false) == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(false)->sensorData().gridCellSize() == 0 || (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data { SensorData tmpData = data; @@ -2629,6 +2714,17 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP } } +void CoreWrapper::envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg) +{ + if(!paused_) + { + // Can only insert one value for each type per node, keep the most recent + EnvSensor value = rtabmap_conversions::envSensorFromROS(*envSensorMsg); + UScopeMutex lock(envSensorMutex_); + uInsert(envSensors_, std::make_pair(value.type(), value)); + } +} + void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection) { if(!paused_) @@ -3117,6 +3213,11 @@ void CoreWrapper::updateRtabmapCallback( } } } + applyParameters(); +} + +void CoreWrapper::applyParameters() +{ RCLCPP_INFO(get_logger(), "rtabmap: Updating parameters"); if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { @@ -3186,6 +3287,9 @@ void CoreWrapper::resetRtabmapCallback( userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + envSensorMutex_.lock(); + envSensors_.clear(); + envSensorMutex_.unlock(); imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); @@ -3251,18 +3355,27 @@ void CoreWrapper::loadDatabaseCallback( // Close old database RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s)...", databasePath_.c_str()); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } - rtabmap_.close(); + rtabmap_.close(saveDatabase); RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); lastPoseMutex_.lock(); @@ -3288,6 +3401,9 @@ void CoreWrapper::loadDatabaseCallback( userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + envSensorMutex_.lock(); + envSensors_.clear(); + envSensorMutex_.unlock(); imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); @@ -3404,18 +3520,27 @@ void CoreWrapper::backupDatabaseCallback( std::shared_ptr) { RCLCPP_INFO(this->get_logger(), "Backup: Saving memory..."); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } - rtabmap_.close(); + rtabmap_.close(saveDatabase); RCLCPP_INFO(this->get_logger(), "Backup: Saving memory... done!"); lastPoseMutex_.lock(); @@ -3436,6 +3561,9 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.unlock(); globalPoses_.clear(); gps_.clear(); + envSensorMutex_.lock(); + envSensors_.clear(); + envSensorMutex_.unlock(); landmarksMutex_.lock(); landmarks_.clear(); landmarksMutex_.unlock(); @@ -3721,9 +3849,9 @@ void CoreWrapper::getNodeDataCallback( req->grid?"true":"false", req->user_data?"true":"false"); - if(req->ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature()) + if(req->ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature(true)) { - req->ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature()->id()); + req->ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature(true)->id()); } for(size_t i=0; iids.size(); ++i) { @@ -3761,6 +3889,8 @@ void CoreWrapper::getMapDataCallback( !req->graph_only, !req->graph_only, !req->graph_only, + !req->graph_only, + !req->graph_only, !req->graph_only); mapToOdomMutex_.lock(); @@ -3787,13 +3917,15 @@ void CoreWrapper::getMapData2Callback( const std::shared_ptr req, std::shared_ptr res) { - RCLCPP_INFO(get_logger(), "rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s)...", + RCLCPP_INFO(get_logger(), "rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s with_words=%s with_global_descriptors=%s)...", req->global_map?"true":"false", req->optimized?"true":"false", req->with_images?"true":"false", req->with_scans?"true":"false", req->with_user_data?"true":"false", - req->with_grids?"true":"false"); + req->with_grids?"true":"false", + req->with_words?"true":"false", + req->with_global_descriptors?"true":"false"); std::map signatures; std::map poses; std::multimap constraints; diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 5bbf39fd..106d23cf 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -55,6 +55,21 @@ include_directories( # libraries SET(Libraries + rclcpp_components::component +) +SET(PublicLibraries + cv_bridge::cv_bridge + rclcpp::rclcpp + message_filters::message_filters + image_transport::image_transport + sensor_msgs::sensor_msgs + nav_msgs::nav_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_conversions::rtabmap_conversions + diagnostic_updater::diagnostic_updater +) + +SET(AmentLibraries cv_bridge image_transport message_filters @@ -67,6 +82,10 @@ SET(Libraries diagnostic_updater ) +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + add_definitions(-DPRE_ROS_LYRICAL) +ENDIF() + ########### ## Build ## ########### @@ -117,8 +136,14 @@ target_include_directories(rtabmap_sync $ ) -ament_target_dependencies(rtabmap_sync ${Libraries}) -ament_target_dependencies(rtabmap_sync_plugins ${Libraries}) + +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_sync ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_sync PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_sync_plugins PUBLIC ${Libraries}) +ENDIF() +target_link_libraries(rtabmap_sync_plugins PUBLIC rtabmap_sync) rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDSync") rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::StereoSync") @@ -126,30 +151,25 @@ rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBSync") rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDXSync") add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp) -ament_target_dependencies(rtabmap_rgbd_sync ${Libraries}) -target_link_libraries(rtabmap_rgbd_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgbd_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync") add_executable(rtabmap_rgbdx_sync src/RGBDXSyncNode.cpp) -ament_target_dependencies(rtabmap_rgbdx_sync ${Libraries}) -target_link_libraries(rtabmap_rgbdx_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgbdx_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgbdx_sync PROPERTIES OUTPUT_NAME "rgbdx_sync") add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp) -ament_target_dependencies(rtabmap_stereo_sync ${Libraries}) -target_link_libraries(rtabmap_stereo_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_stereo_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync") add_executable(rtabmap_rgb_sync src/RGBSyncNode.cpp) -ament_target_dependencies(rtabmap_rgb_sync ${Libraries}) -target_link_libraries(rtabmap_rgb_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgb_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgb_sync PROPERTIES OUTPUT_NAME "rgb_sync") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_sync rtabmap_sync_plugins) # To include downstream without targets diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 1f3f63b2..8fb31a6d 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -269,6 +269,8 @@ private: std::string odomFrameId_; int rgbdCameras_; std::string name_; + std::string imageTransport_; + std::string depthTransport_; rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 03b49348..f9411519 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -62,7 +62,7 @@ class SyncDiagnostic { diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } - void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { updateFrequency( stamp, @@ -74,9 +74,12 @@ class SyncDiagnostic { lastTickInputStamp_); } - void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { - double lastTickOutputStamp; + if(expectedFrequency == 0.0) { + outTargetFrequency_ = inTargetFrequency_; + } + double lastTickOutputStamp = 0.0; updateFrequency( stamp, expectedFrequency, @@ -112,31 +115,33 @@ private: timeStatus.tick(stamp); double stampSec = rtabmap_conversions::timestampFromROS(stamp); - double singlePeriod = stampSec - lastTickStamp; - window.push_back(singlePeriod); - if(window.size() > windowSize_) + if(expectedFrequency>0) { - window.pop_front(); + targetFrequency = expectedFrequency; + } + else if(lastTickStamp > 0.0) { + double singlePeriod = stampSec - lastTickStamp; - double period = 0.0; - if(window.size() == windowSize_) + window.push_back(singlePeriod); + 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; - + if(period>0.0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) + { + targetFrequency = 1.0/period; + } } } diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp index 204d7370..571ef3a1 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp @@ -58,6 +58,7 @@ public: private: double compressedRate_; + bool fillEmptyDepth_; rclcpp::Time lastCompressedPublished_; diff --git a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp index 3cf545e5..8f24b785 100644 --- a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp @@ -59,6 +59,7 @@ public: const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight); private: double compressedRate_; + double approxSyncMaxInterval_; rclcpp::Time lastCompressedPublished_; rclcpp::Publisher::SharedPtr rgbdImagePub_; diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index cd766488..39baf6b4 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -2,7 +2,7 @@ rtabmap_sync - 0.22.1 + 0.23.7 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index d1370c8e..c8cbfb06 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -47,6 +47,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : subscribedToUserData_(false), odomFrameId_(""), rgbdCameras_(1), + imageTransport_("raw"), + depthTransport_("raw"), // RGB + Depth SYNC_INIT(depth), @@ -392,6 +394,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : "\"sync_queue_size\".", syncQueueSize_); } syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_); + imageTransport_ = node.declare_parameter("image_transport", imageTransport_); + depthTransport_ = node.declare_parameter("depth_transport", depthTransport_); int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT); int qosOdom = node.declare_parameter("qos_odom", qos); @@ -527,6 +531,13 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: subscribe_stereo = %s", name_.c_str(), subscribedToStereo_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name_.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras_); RCLCPP_INFO(node.get_logger(), "%s: subscribe_sensor_data = %s", name_.c_str(), subscribedToSensorData_?"true":"false"); + if(subscribedToOdom_ && !odomFrameId_.empty()) { + RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = false (\"odom_frame_id\" is set)", name_.c_str()); + subscribedToOdom_ = false; + } + else { + RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = %s", name_.c_str(), subscribedToOdom_?"true":"false"); + } RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom_info = %s", name_.c_str(), subscribedToOdomInfo_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_user_data = %s", name_.c_str(), subscribedToUserData_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false"); @@ -540,11 +551,12 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: qos_odom = %d", name_.c_str(), qosOdom_); 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_INFO(node.get_logger(), "%s: image_transport = %s", name_.c_str(), imageTransport_.c_str()); + RCLCPP_INFO(node.get_logger(), "%s: depth_transport = %s", name_.c_str(), depthTransport_.c_str()); rclcpp::SubscriptionOptions callbackOptions; callbackOptions.callback_group = syncCallbackGroup_; - subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_; if(subscribedToDepth_) { setupDepthCallbacks( diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 4370e9cc..b32bee80 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -503,9 +503,19 @@ void CommonDataSubscriber::setupDepthCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup depth callback"); - image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); - imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + std::string rgbTopic = node.get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = node.get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints rgbHints(&node); // using "image_transport" parameter + image_transport::TransportHints depthHints(&node, "raw", "depth_transport"); + imageSub_.subscribe(&node, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageDepthSub_.subscribe(&node, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); +#else + image_transport::TransportHints rgbHints(node); // using "image_transport" parameter + image_transport::TransportHints depthHints(node, "raw", "depth_transport"); + imageSub_.subscribe(node, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_), options); + imageDepthSub_.subscribe(node, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_), options); +#endif cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); #ifdef RTABMAP_SYNC_USER_DATA diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index 9665e223..56354481 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -503,8 +503,14 @@ void CommonDataSubscriber::setupRGBCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); - image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + std::string rgbTopic = node.get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(&node); // using "image_transport" parameter + imageSub_.subscribe(&node, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); +#else + image_transport::TransportHints hints(node); // using "image_transport" parameter + imageSub_.subscribe(node, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_), options); +#endif cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); #ifdef RTABMAP_SYNC_USER_DATA diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 8ddd3e65..b6a89724 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -97,9 +97,17 @@ void CommonDataSubscriber::setupStereoCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); - image_transport::TransportHints hints(&node); - imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); - imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + std::string leftTopic = node.get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = node.get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(&node); // using "image_transport" parameter + imageRectLeft_.subscribe(&node, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(&node, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); +#else + image_transport::TransportHints hints(node); // using "image_transport" parameter + imageRectLeft_.subscribe(node, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_), options); + imageRectRight_.subscribe(node, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_), options); +#endif cameraInfoLeft_.subscribe(&node, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); cameraInfoRight_.subscribe(&node, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 9f249390..585d85ad 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -48,6 +48,7 @@ namespace rtabmap_sync RGBSync::RGBSync(const rclcpp::NodeOptions & options) : Node("rgbd_sync", options), compressedRate_(0), + fillEmptyDepth_(false), approxSync_(0), exactSync_(0) { @@ -72,6 +73,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCaminfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); + fillEmptyDepth_ = this->declare_parameter("fill_empty_depth", fillEmptyDepth_); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) @@ -81,6 +84,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : 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_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "%s: fill_empty_depth = %s", get_name(), fillEmptyDepth_?"true":"false"); rgbdImagePub_ = this->create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -98,8 +103,14 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : 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(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(this); // using "image_transport" parameter + imageSub_.subscribe(this, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + image_transport::TransportHints hints(*this); // using "image_transport" parameter + imageSub_.subscribe(*this, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCaminfo)); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", @@ -147,6 +158,14 @@ void RGBSync::callback( msg.header.frame_id = cameraInfo->header.frame_id; msg.header.stamp = image->header.stamp; msg.rgb_camera_info = *cameraInfo; + cv_bridge::CvImage fakeDepthImage; + if(fillEmptyDepth_) + { + msg.depth_camera_info = *cameraInfo; + fakeDepthImage.header = image->header; + fakeDepthImage.image = cv::Mat::zeros(image->height, image->width, CV_16UC1); + fakeDepthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } if(rgbdImageCompressedPub_->get_subscription_count()) { @@ -169,6 +188,13 @@ void RGBSync::callback( cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG); + if(fillEmptyDepth_) + { + msgCompressed.depth_compressed.header = image->header; + msgCompressed.depth_compressed.data = rtabmap::compressImage(fakeDepthImage.image, ".png"); + msgCompressed.depth_compressed.format = "png"; + } + rgbdImageCompressedPub_->publish(msgCompressed); } } @@ -176,6 +202,10 @@ void RGBSync::callback( if(rgbdImagePub_->get_subscription_count()) { msg.rgb = *image; + if(fillEmptyDepth_) + { + fakeDepthImage.toImageMsg(msg.depth); + } rgbdImagePub_->publish(msg); } diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 9aecf358..7c73a5af 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -76,6 +76,22 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_ = this->declare_parameter("depth_scale", depthScale_); decimation_ = this->declare_parameter("decimation", decimation_); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", std::string("raw")); + std::string depthImageTransport = this->declare_parameter("depth_image_transport", std::string("raw")); + if(rgbImageTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"rgb_image_transport\" has been renamed " + "to \"image_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"image_transport\".", rgbImageTransport.c_str()); + } + if(depthImageTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"depth_image_transport\" has been renamed " + "to \"depth_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"depth_transport\".", depthImageTransport.c_str()); + } + std::string imageTransport = this->declare_parameter("image_transport", rgbImageTransport); + std::string depthTransport = this->declare_parameter("depth_transport", depthImageTransport); if(decimation_<1) { @@ -92,6 +108,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_); RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "%s: depth_transport = %s", get_name(), depthTransport.c_str()); rgbdImagePub_ = this->create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -109,12 +127,19 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } - 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()); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints rgbHints(this); // using "image_transport" parameter + image_transport::TransportHints depthHints(this, "raw", "depth_transport"); + imageSub_.subscribe(this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + image_transport::TransportHints rgbHints(*this); // using "image_transport" parameter + image_transport::TransportHints depthHints(*this, "raw", "depth_transport"); + imageSub_.subscribe(*this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); + imageDepthSub_.subscribe(*this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", @@ -197,8 +222,14 @@ void RGBDSync::callback( cv::Mat rgbMat; cv::Mat depthMat; - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv_bridge::CvImageConstPtr imagePtr, imageDepthPtr; + try { + imagePtr = cv_bridge::toCvShare(image); + imageDepthPtr = cv_bridge::toCvShare(depth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rgbMat = imagePtr->image; depthMat = imageDepthPtr->image; diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index 9714073b..e2377516 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -33,7 +33,7 @@ namespace rtabmap_sync { RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : - Node("rgbd_sync", options), + Node("rgbdx_sync", options), SYNC_INIT(rgbd2), SYNC_INIT(rgbd3), SYNC_INIT(rgbd4), diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index a1b4767c..feb831a1 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -47,16 +47,16 @@ namespace rtabmap_sync StereoSync::StereoSync(const rclcpp::NodeOptions & options) : Node("stereo_sync", options), compressedRate_(0), + approxSyncMaxInterval_(0.0), approxSync_(0), exactSync_(0) { int topicQueueSize = 10; int syncQueueSize = 10; bool approxSync = false; - double approxSyncMaxInterval = 0.0; int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; approxSync = this->declare_parameter("approx_sync", approxSync); - approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval); + approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_); topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize); int queueSize = this->declare_parameter("queue_size", -1); if(queueSize != -1) @@ -71,14 +71,16 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); 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: 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: compressed_rate = %f", get_name(), compressedRate_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); rgbdImagePub_ = create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -86,8 +88,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); - if(approxSyncMaxInterval>0.0) - approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval)); + if(approxSyncMaxInterval_>0.0) + approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_)); approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } else @@ -96,16 +98,24 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : 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(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()); + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(this); // using "image_transport" parameter + imageLeftSub_.subscribe(this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRightSub_.subscribe(this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + image_transport::TransportHints hints(*this); // using "image_transport" parameter + imageLeftSub_.subscribe(*this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); + imageRightSub_.subscribe(*this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoLeftSub_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRightSub_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", get_name(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageLeftSub_.getSubscriber().getTopic().c_str(), imageRightSub_.getSubscriber().getTopic().c_str(), cameraInfoLeftSub_.getSubscriber()->get_topic_name(), @@ -181,10 +191,22 @@ void StereoSync::callback( rtabmap_msgs::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_msgs::msg::RGBDImage); *msgCompressed = *msg; - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft); + cv_bridge::CvImageConstPtr imagePtr; + try { + imagePtr = cv_bridge::toCvShare(imageLeft); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight); + cv_bridge::CvImageConstPtr imageDepthPtr; + try { + imageDepthPtr = cv_bridge::toCvShare(imageRight); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } imageDepthPtr->toCompressedImageMsg(msgCompressed->depth_compressed, cv_bridge::JPG); rgbdImageCompressedPub_->publish(std::move(msgCompressed)); diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 525d239e..718e0456 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -26,6 +26,9 @@ find_package(pcl_ros REQUIRED) find_package(message_filters REQUIRED) find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_conversions REQUIRED) +find_package(rtabmap_sync REQUIRED) + +find_package(RTABMap COMPONENTS gui REQUIRED) # Optional components find_package(octomap_msgs) @@ -37,6 +40,29 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + image_transport::image_transport + rclcpp_components::component + stereo_msgs::stereo_msgs + std_msgs::std_msgs + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + laser_geometry::laser_geometry + image_geometry::image_geometry + message_filters::message_filters + rtabmap_msgs::rtabmap_msgs + rtabmap_sync::rtabmap_sync +) +SET(PublicLibraries + rclcpp::rclcpp + nav_msgs::nav_msgs + pcl_conversions::pcl_conversions + sensor_msgs::sensor_msgs + rtabmap_conversions::rtabmap_conversions +) + +SET(AmentLibraries cv_bridge image_transport rclcpp @@ -54,18 +80,24 @@ SET(Libraries message_filters rtabmap_msgs rtabmap_conversions + rtabmap_sync ) if("$ENV{ROS_DISTRO}" STRLESS "jazzy") add_definitions(-DPRE_ROS_JAZZY) endif() +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + add_definitions(-DPRE_ROS_LYRICAL) +ENDIF() + ########### ## Build ## ########### - -SET(rtabmap_util_plugins_lib_src - src/MapsManager.cpp +SET(rtabmap_util_lib_src + src/MapsManager.cpp +) +SET(rtabmap_util_plugins_src src/nodelets/point_cloud_xyzrgb.cpp src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp @@ -74,22 +106,23 @@ SET(rtabmap_util_plugins_lib_src src/nodelets/point_cloud_aggregator.cpp src/nodelets/point_cloud_assembler.cpp src/nodelets/imu_to_tf.cpp + src/nodelets/db_player.cpp src/nodelets/lidar_deskewing.cpp src/nodelets/rgbd_relay.cpp src/nodelets/rgbd_split.cpp src/nodelets/map_assembler.cpp ) - # If octomap is found, add dependency IF(octomap_msgs_FOUND) MESSAGE(STATUS "WITH octomap_msgs") -include_directories( - ${octomap_msgs_INCLUDE_DIRS} +SET(PublicLibraries + octomap_msgs::octomap_msgs + ${PublicLibraries} ) -SET(Libraries +SET(AmentLibraries octomap_msgs - ${Libraries} + ${AmentLibraries} ) ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) @@ -97,41 +130,52 @@ ENDIF(octomap_msgs_FOUND) # If grid_map is found, add dependency IF(grid_map_ros_FOUND) MESSAGE(STATUS "WITH grid_map_ros") -include_directories( - ${grid_map_ros_INCLUDE_DIRS} +SET(PublicLibraries + grid_map_ros::grid_map_ros + ${PublicLibraries} ) -SET(Libraries +SET(AmentLibraries grid_map_ros - ${Libraries} + ${AmentLibraries} ) ENDIF(grid_map_ros_FOUND) ############################ ## Declare a cpp library ############################ -add_library(rtabmap_util_plugins SHARED - ${rtabmap_util_plugins_lib_src} +add_library(rtabmap_util SHARED + ${rtabmap_util_lib_src} ) -target_include_directories(rtabmap_util_plugins +add_library(rtabmap_util_plugins SHARED + ${rtabmap_util_plugins_src} +) +target_include_directories(rtabmap_util PUBLIC $ $ ) IF(octomap_msgs_FOUND) - target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_OCTOMAP_MSGS) + target_compile_definitions(rtabmap_util PUBLIC -DWITH_OCTOMAP_MSGS) ENDIF(octomap_msgs_FOUND) IF(grid_map_ros_FOUND) - target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_GRID_MAP_ROS) + target_compile_definitions(rtabmap_util PUBLIC -DWITH_GRID_MAP_ROS) ENDIF(grid_map_ros_FOUND) -ament_target_dependencies(rtabmap_util_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_util ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_util PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_util_plugins PUBLIC ${Libraries}) +ENDIF() +target_link_libraries(rtabmap_util_plugins PUBLIC rtabmap_util) rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DisparityToDepth") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ImuToTF") +rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DbPlayer") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::LidarDeskewing") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZ") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZRGB") @@ -142,93 +186,78 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudA 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}) -target_link_libraries(rtabmap_rgbd_relay rtabmap_util_plugins) +target_link_libraries(rtabmap_rgbd_relay PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay") add_executable(rtabmap_rgbd_split src/RGBDSplitNode.cpp) -ament_target_dependencies(rtabmap_rgbd_split ${Libraries}) -target_link_libraries(rtabmap_rgbd_split rtabmap_util_plugins) +target_link_libraries(rtabmap_rgbd_split PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") #add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp) -#ament_target_dependencies(rtabmap_map_optimizer ${Libraries}) #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) +target_link_libraries(rtabmap_map_assembler PRIVATE 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}) -target_link_libraries(rtabmap_imu_to_tf rtabmap_util_plugins) +target_link_libraries(rtabmap_imu_to_tf PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") add_executable(rtabmap_disparity_to_depth src/DisparityToDepthNode.cpp) -ament_target_dependencies(rtabmap_disparity_to_depth ${Libraries}) -target_link_libraries(rtabmap_disparity_to_depth rtabmap_util_plugins) +target_link_libraries(rtabmap_disparity_to_depth PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_disparity_to_depth PROPERTIES OUTPUT_NAME "disparity_to_depth") add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp) -ament_target_dependencies(rtabmap_lidar_deskewing ${Libraries}) -target_link_libraries(rtabmap_lidar_deskewing rtabmap_util_plugins) +target_link_libraries(rtabmap_lidar_deskewing PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing") add_executable(rtabmap_point_cloud_xyz src/PointCloudXYZNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_xyz ${Libraries}) -target_link_libraries(rtabmap_point_cloud_xyz rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_xyz PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_xyz PROPERTIES OUTPUT_NAME "point_cloud_xyz") add_executable(rtabmap_point_cloud_xyzrgb src/PointCloudXYZRGBNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_xyzrgb ${Libraries}) -target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_xyzrgb PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb") -#add_executable(rtabmap_data_player src/DbPlayerNode.cpp) -#ament_target_dependencies(rtabmap_data_player ${Libraries}) -#target_link_libraries(rtabmap_data_player rtabmap_util_plugins) -#set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") +add_executable(rtabmap_data_player src/DbPlayerNode.cpp) +target_link_libraries(rtabmap_data_player PRIVATE rtabmap_util_plugins) +set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") #add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp) -#ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries}) -#target_link_libraries(rtabmap_odom_msg_to_tf rtabmap_util_plugins) +#target_link_libraries(rtabmap_odom_msg_to_tf PRIVATE ${Libraries}) +#target_link_libraries(rtabmap_odom_msg_to_tf PRIVATE rtabmap_util_plugins) #set_target_properties(rtabmap_odom_msg_to_tf PROPERTIES OUTPUT_NAME "odom_msg_to_tf") add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.cpp) -ament_target_dependencies(rtabmap_pointcloud_to_depthimage ${Libraries}) -target_link_libraries(rtabmap_pointcloud_to_depthimage rtabmap_util_plugins) +target_link_libraries(rtabmap_pointcloud_to_depthimage PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage") add_executable(rtabmap_obstacles_detection src/ObstaclesDetectionNode.cpp) -ament_target_dependencies(rtabmap_obstacles_detection ${Libraries}) -target_link_libraries(rtabmap_obstacles_detection rtabmap_util_plugins) +target_link_libraries(rtabmap_obstacles_detection PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_obstacles_detection PROPERTIES OUTPUT_NAME "obstacles_detection") add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_aggregator ${Libraries}) -target_link_libraries(rtabmap_point_cloud_aggregator rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_aggregator PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator") add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_assembler ${Libraries}) -target_link_libraries(rtabmap_point_cloud_assembler rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_assembler PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets -ament_export_libraries(rtabmap_util_plugins) # To include downstream without targets +ament_export_libraries(rtabmap_util rtabmap_util_plugins) # To include downstream without targets # Install Python executables install(PROGRAMS -# scripts/patrol.py + scripts/patrol.py # scripts/objects_to_tags.py # scripts/point_to_tf.py # scripts/netvlad_tf_ros.py @@ -240,6 +269,7 @@ install(PROGRAMS ) install(TARGETS + rtabmap_util rtabmap_util_plugins EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib @@ -248,7 +278,7 @@ install(TARGETS ) install(TARGETS # rtabmap_map_optimizer -# rtabmap_data_player + rtabmap_data_player # rtabmap_odom_msg_to_tf rtabmap_imu_to_tf rtabmap_disparity_to_depth diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index ee41af8c..f6dd3c61 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -111,6 +111,7 @@ private: bool mapCacheCleanup_; bool alwaysUpdateMap_; bool scanEmptyRayTracing_; + bool localMapsCacheLoadedOnInit_; rclcpp::Publisher::SharedPtr cloudMapPub_; rclcpp::Publisher::SharedPtr cloudGroundPub_; diff --git a/rtabmap_util/include/rtabmap_util/db_player.hpp b/rtabmap_util/include/rtabmap_util/db_player.hpp new file mode 100644 index 00000000..44c02d0f --- /dev/null +++ b/rtabmap_util/include/rtabmap_util/db_player.hpp @@ -0,0 +1,123 @@ +/* +Copyright (c) 2010-2025, 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 +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace rtabmap_util +{ + +class DbPlayer : public rclcpp::Node +{ +public: + RTABMAP_UTIL_PUBLIC + explicit DbPlayer(const rclcpp::NodeOptions & options); + virtual ~DbPlayer(); + bool publishNextFrame(); + bool isPaused() const {return paused_;} + void setPaused(bool enabled) {paused_ = enabled;} + +private: + void pauseCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + void resumeCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + void initializePublishers(const rtabmap::OdometryEvent & odom); + bool cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage); + +private: + bool paused_; + std::shared_ptr reader_; + std::string frameId_; + std::string odomFrameId_; + std::string cameraFrameId_; + std::string scanFrameId_; + std::string gtFrameId_; + std::string gtBaseFrameId_; + std::string imuFrameId_; + int qos_; + int qosCameraInfo_; + int qosOdom_; + int qosScan_; + int qosScanCloud_; + int qosGlobalPose_; + int qosGps_; + int qosImu_; + int qosEnvSensor_; + double scanAngleMin_; + double scanAngleMax_; + double scanAngleIncrement_; + double scanRangeMin_; + double scanRangeMax_; + + rclcpp::Service::SharedPtr pauseSrv_; + rclcpp::Service::SharedPtr resumeSrv_; + + image_transport::Publisher imagePub_; + image_transport::Publisher rgbPub_; + image_transport::Publisher depthPub_; + image_transport::Publisher leftPub_; + image_transport::Publisher rightPub_; + rclcpp::Publisher::SharedPtr rgbInfoPub_; + rclcpp::Publisher::SharedPtr depthInfoPub_; + rclcpp::Publisher::SharedPtr leftInfoPub_; + rclcpp::Publisher::SharedPtr rightInfoPub_; + std::vector::SharedPtr> rgbdImagePubs_; + rclcpp::Publisher::SharedPtr odometryPub_; + rclcpp::Publisher::SharedPtr scanPub_; + rclcpp::Publisher::SharedPtr scanCloudPub_; + rclcpp::Publisher::SharedPtr globalPosePub_; + rclcpp::Publisher::SharedPtr gpsFixPub_; + rclcpp::Publisher::SharedPtr envSensorPub_; + rclcpp::Publisher::SharedPtr imuPub_; + rclcpp::Publisher::SharedPtr clockPub_; + std::shared_ptr tfBroadcaster_; +}; + +} diff --git a/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp b/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp index 9622ba7b..5706961d 100644 --- a/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp +++ b/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp @@ -30,9 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include +#include +#include +#include namespace rtabmap_util { diff --git a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp index 3f19b09c..706eb10d 100644 --- a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp +++ b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp @@ -28,8 +28,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rclcpp/rclcpp.hpp" -#include -#include +#include + +#include +#include #include #include @@ -58,6 +60,8 @@ private: bool slerp_; std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; + std::unique_ptr scanSyncDiagnostic_; + std::unique_ptr cloudSyncDiagnostic_; }; } diff --git a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp index 9610886a..b85dc7e7 100644 --- a/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp +++ b/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp @@ -28,8 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rclcpp/rclcpp.hpp" #include -#include -#include +#include +#include #include diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp index 8c62fe79..ff72354a 100644 --- a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp +++ b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp @@ -30,8 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include +#include +#include #include #include diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp index 252970f3..cc756894 100644 --- a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp +++ b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp @@ -28,8 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include #include #include diff --git a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp index ff408984..7896986b 100644 --- a/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp +++ b/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp @@ -34,8 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include +#include +#include #include #include diff --git a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp index a735dedf..d31a736a 100644 --- a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp +++ b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp @@ -50,8 +50,10 @@ public: private: rclcpp::Subscription::SharedPtr rgbdImageSub_; - image_transport::CameraPublisher rgbPub_; - image_transport::CameraPublisher depthPub_; + image_transport::Publisher rgbPub_; + image_transport::Publisher depthPub_; + rclcpp::Publisher::SharedPtr rgbInfoPub_; + rclcpp::Publisher::SharedPtr depthInfoPub_; }; } diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 9d0068a7..35bc2f09 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -2,7 +2,7 @@ rtabmap_util - 0.22.1 + 0.23.7 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe @@ -27,12 +27,14 @@ tf2_geometry_msgs tf2_ros laser_geometry + image_geometry pcl_conversions pcl_ros message_filters rtabmap_msgs rtabmap_conversions - + rtabmap_sync + grid_map_ros ament_cmake diff --git a/rtabmap_util/scripts/netvlad_tf_ros.py b/rtabmap_util/scripts/netvlad_tf_ros.py index 4b1c816d..32b45b08 100755 --- a/rtabmap_util/scripts/netvlad_tf_ros.py +++ b/rtabmap_util/scripts/netvlad_tf_ros.py @@ -24,7 +24,7 @@ import netvlad_tf.nets as nets from std_msgs.msg import String from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError -from rtabmap_python import compression as cp +from rtabmap_python import cv_compression as cp from rtabmap_msgs.msg import GlobalDescriptor class netvlad_ros: diff --git a/rtabmap_util/scripts/patrol.py b/rtabmap_util/scripts/patrol.py index ee131258..e0252a29 100755 --- a/rtabmap_util/scripts/patrol.py +++ b/rtabmap_util/scripts/patrol.py @@ -1,87 +1,104 @@ -#!/usr/bin/env python -import rospy +#!/usr/bin/env python3 import sys +import time +import rclpy +from rclpy.node import Node from std_msgs.msg import Bool -from rtabmap_ros.msg import Goal +from rtabmap_msgs.msg import Goal -pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1) -waypoints = [] -currentIndex = 0 -waitingTime = 1.0 -frameId = "" -def callback(data): - global currentIndex - global waitingTime - global frameId - if data.data: - rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) - else: - rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) +class PatrolNode(Node): + def __init__(self, waypoints): + super().__init__('patrol') - currentIndex = (currentIndex+1) % len(waypoints) + # --- Parameters --- + self.declare_parameter('time', 1.0) + self.declare_parameter('frame_id', '') + self.waiting_time = self.get_parameter('time').value + self.frame_id = self.get_parameter('frame_id').value - # Waiting time before sending next goal - rospy.sleep(waitingTime) + # --- Variables --- + self.waypoints = waypoints + self.current_index = 0 + + # --- Publisher & Subscriber --- + self.pub = self.create_publisher(Goal, 'rtabmap/goal_node', 10) + self.sub = self.create_subscription(Bool, 'rtabmap/goal_reached', self.callback, 10) + + self.get_logger().info(f"Waypoints: {self.waypoints}") + self.get_logger().info(f"Waiting time: {self.waiting_time:.1f} sec") + self.get_logger().info(f"Publishing goals on: {self.pub.topic_name}") + self.get_logger().info(f"Receiving goal status on: {self.sub.topic_name}") + + # Delay before sending first goal (ensure discovery) + time.sleep(1.0) + + # Send first goal + self.send_goal() + + def callback(self, msg: Bool): + """Called when goal_reached is received.""" + if msg.data: + self.get_logger().info( + f"Goal '{self.waypoints[self.current_index]}' reached! " + f"Publishing next goal in {self.waiting_time:.1f} sec..." + ) + else: + self.get_logger().info( + f"Goal '{self.waypoints[self.current_index]}' failed! " + f"Publishing next goal in {self.waiting_time:.1f} sec..." + ) + + # Move to next waypoint + self.current_index = (self.current_index + 1) % len(self.waypoints) + + # Wait before sending next goal + time.sleep(self.waiting_time) + self.send_goal() + + def send_goal(self): + """Send current goal to RTAB-Map.""" + waypoint = self.waypoints[self.current_index] + msg = Goal() + msg.header.stamp = self.get_clock().now().to_msg() + msg.frame_id = self.frame_id + + # Check if waypoint is a node id (int) or a label (string) + try: + msg.node_id = int(waypoint) + msg.node_label = "" + except ValueError: + msg.node_id = 0 + msg.node_label = waypoint + + self.get_logger().info( + f"Publishing goal '{waypoint}' ({self.current_index + 1}/{len(self.waypoints)})" + ) + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + # Extract waypoints from command-line args + if len(sys.argv) < 3: + print( + "Usage: patrol.py waypointA waypointB waypointC ... " + "[--ros-args -p time:=1.0 -p frame_id:=base_footprint]" + ) + return + + waypoints = [x for x in sys.argv[1:] if not x.startswith('--') and not x.startswith('_')] + node = PatrolNode(waypoints) - msg = Goal() - msg.frame_id = frameId try: - int(waypoints[currentIndex]) - is_dig = True - except ValueError: - is_dig = False - if is_dig: - msg.node_id = int(waypoints[currentIndex]) - msg.node_label = "" - else: - msg.node_id = 0 - msg.node_label = waypoints[currentIndex] - - rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) - msg.header.stamp = rospy.get_rostime() - pub.publish(msg) + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() -def main(): - rospy.init_node('patrol', anonymous=False) - sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback) - global waitingTime - global frameId - waitingTime = rospy.get_param('~time', waitingTime) - frameId = rospy.get_param('~frame_id', frameId) - rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal - - rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]')) - rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime) - rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name) - rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name) - - # send the first goal - msg = Goal() - msg.frame_id = frameId - try: - int(waypoints[currentIndex]) - is_dig = True - except ValueError: - is_dig = False - if is_dig: - msg.node_id = int(waypoints[currentIndex]) - msg.node_label = "" - else: - msg.node_id = 0 - msg.node_label = waypoints[currentIndex] - while rospy.Time.now().secs == 0: - rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...") - rospy.sleep(.1) - msg.header.stamp = rospy.Time.now() - rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) - pub.publish(msg) - rospy.spin() if __name__ == '__main__': - if len(sys.argv) < 3: - print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)") - else: - waypoints = sys.argv[1:] - waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')] - main() + main() \ No newline at end of file diff --git a/rtabmap_util/scripts/yaml_to_camera_info.py b/rtabmap_util/scripts/yaml_to_camera_info.py index 88bec9f1..5e822c03 100755 --- a/rtabmap_util/scripts/yaml_to_camera_info.py +++ b/rtabmap_util/scripts/yaml_to_camera_info.py @@ -73,9 +73,14 @@ class YamlToCameraInfo(Node): def main(args=None): rclpy.init(args=args) yaml_to_camera_info = YamlToCameraInfo() - rclpy.spin(yaml_to_camera_info) - yaml_to_camera_info.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(yaml_to_camera_info) + except KeyboardInterrupt: + pass + finally: + yaml_to_camera_info.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": main() diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 0650d810..cd892cea 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,36 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef PRE_ROS_IRON -#include -#else -#include -#endif -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rtabmap_util/db_player.hpp" +#include "rtabmap/utilite/ULogger.h" +#include "rclcpp/rclcpp.hpp" #ifndef _WIN32 #include @@ -96,591 +69,61 @@ bool spacehit() } #endif -bool paused = false; -bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +int main(int argc, char **argv) { - if(paused) - { - ROS_WARN("Already paused!"); - } - else - { - paused = true; - ROS_INFO("paused!"); - } - return true; -} + ULogger::setType(ULogger::kTypeConsole); -bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - if(!paused) - { - ROS_WARN("Already running!"); - } - else - { - paused = false; - ROS_INFO("resumed!"); - } - return true; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "data_player"); - - //ULogger::setType(ULogger::kTypeConsole); - //ULogger::setLevel(ULogger::kDebug); - //ULogger::setEventLevel(ULogger::kWarning); - - bool publishClock = false; + std::vector arguments; for(int i=1;i(options); - pnh.param("frame_id", frameId, frameId); - pnh.param("odom_frame_id", odomFrameId, odomFrameId); - pnh.param("camera_frame_id", cameraFrameId, cameraFrameId); - pnh.param("scan_frame_id", scanFrameId, scanFrameId); - pnh.param("rate", rate, rate); // Ratio of the database stamps - pnh.param("database", databasePath, databasePath); - pnh.param("publish_tf", publishTf, publishTf); - pnh.param("start_id", startId, startId); + rclcpp::Rate pauseRate(10); - // A general 360 lidar with 0.5 deg increment - double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax; - pnh.param("scan_angle_min", scanAngleMin, -M_PI); - pnh.param("scan_angle_max", scanAngleMax, M_PI); - pnh.param("scan_angle_increment", scanAngleIncrement, M_PI / 720.0); - pnh.param("scan_range_min", scanRangeMin, 0.0); - pnh.param("scan_range_max", scanRangeMax, 60); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); - ROS_INFO("frame_id = %s", frameId.c_str()); - ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); - ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); - ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); - ROS_INFO("rate = %f", rate); - ROS_INFO("publish_tf = %s", publishTf?"true":"false"); - ROS_INFO("start_id = %d", startId); - ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false"); - - if(databasePath.empty()) + while(rclcpp::ok()) { - ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database)."); - return -1; - } - databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); - if(databasePath.size() && databasePath.at(0) != '/') - { - databasePath = UDirectory::currentDir(true) + databasePath; - } - ROS_INFO("database = %s", databasePath.c_str()); - - rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId); - if(!reader.init()) - { - ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str()); - return -1; - } - - ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback); - ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback); - - image_transport::ImageTransport it(nh); - image_transport::Publisher imagePub; - image_transport::Publisher rgbPub; - image_transport::Publisher depthPub; - image_transport::Publisher leftPub; - image_transport::Publisher rightPub; - ros::Publisher rgbCamInfoPub; - ros::Publisher depthCamInfoPub; - ros::Publisher leftCamInfoPub; - ros::Publisher rightCamInfoPub; - ros::Publisher odometryPub; - ros::Publisher scanPub; - ros::Publisher scanCloudPub; - ros::Publisher globalPosePub; - ros::Publisher gpsFixPub; - ros::Publisher clockPub; - tf2_ros::TransformBroadcaster tfBroadcaster; - - if(publishClock) - { - clockPub = nh.advertise("/clock", 1); - } - - UTimer timer; - rtabmap::CameraInfo cameraInfo; - rtabmap::SensorData data = reader.takeImage(&cameraInfo); - rtabmap::OdometryInfo odomInfo; - odomInfo.reg.covariance = cameraInfo.odomCovariance; - rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); - double acquisitionTime = timer.ticks(); - while(ros::ok() && odom.data().id()) - { - ROS_INFO("Reading sensor data %d...", odom.data().id()); - - ros::Time time(odom.data().stamp()); - - if(publishClock) - { - rosgraph_msgs::Clock msg; - msg.clock = time; - clockPub.publish(msg); + if(!node->publishNextFrame()) { + // end of file, exit + RCLCPP_INFO(node->get_logger(), "Last frame published, exiting!"); + break; } - sensor_msgs::CameraInfo camInfoA; //rgb or left - sensor_msgs::CameraInfo camInfoB; //depth or right - - camInfoA.K.assign(0); - camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1; - camInfoA.R.assign(0); - camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1; - camInfoA.P.assign(0); - camInfoA.P[10] = 1; - - camInfoA.header.frame_id = cameraFrameId; - camInfoA.header.stamp = time; - - camInfoB = camInfoA; - - int type = -1; - if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) - { - if(odom.data().cameraModels().size() > 1) - { - ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); - } - else - { - //depth - if(odom.data().cameraModels().size()) - { - camInfoA.D.resize(5,0); - - camInfoA.P[0] = odom.data().cameraModels()[0].fx(); - camInfoA.K[0] = odom.data().cameraModels()[0].fx(); - camInfoA.P[5] = odom.data().cameraModels()[0].fy(); - camInfoA.K[4] = odom.data().cameraModels()[0].fy(); - camInfoA.P[2] = odom.data().cameraModels()[0].cx(); - camInfoA.K[2] = odom.data().cameraModels()[0].cx(); - camInfoA.P[6] = odom.data().cameraModels()[0].cy(); - camInfoA.K[5] = odom.data().cameraModels()[0].cy(); - - camInfoB = camInfoA; - } - - type=0; - - if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1); - if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1); - if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise("rgb/camera_info", 1); - if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise("depth_registered/camera_info", 1); - } - } - else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) - { - if(odom.data().stereoCameraModels().size() > 1) - { - ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); - } - else - { - //stereo - if(odom.data().stereoCameraModels()[0].isValidForProjection()) - { - camInfoA.D.resize(8,0); - - camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy(); - camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy(); - - camInfoB = camInfoA; - camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx - } - - type=1; - - if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1); - if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1); - if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise("left/camera_info", 1); - if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise("right/camera_info", 1); - } - - } - else - { - if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1); - } - - camInfoA.height = odom.data().imageRaw().rows; - camInfoA.width = odom.data().imageRaw().cols; - camInfoB.height = odom.data().depthOrRightRaw().rows; - camInfoB.width = odom.data().depthOrRightRaw().cols; - - if(!odom.data().laserScanRaw().isEmpty()) - { - if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d()) - { - scanPub = nh.advertise("scan", 1); - if(odom.data().laserScanRaw().angleIncrement() > 0.0f) - { - ROS_INFO("Scan will be published."); - } - else - { - ROS_INFO("Scan will be published with those parameters:"); - ROS_INFO(" scan_angle_min=%f", scanAngleMin); - ROS_INFO(" scan_angle_max=%f", scanAngleMax); - ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement); - ROS_INFO(" scan_range_min=%f", scanRangeMin); - ROS_INFO(" scan_range_max=%f", scanRangeMax); - } - } - else if(scanCloudPub.getTopic().empty()) - { - scanCloudPub = nh.advertise("scan_cloud", 1); - ROS_INFO("Scan cloud will be published."); - } - } - - if(!odom.data().globalPose().isNull() && - odom.data().globalPoseCovariance().cols==6 && - odom.data().globalPoseCovariance().rows==6) - { - if(globalPosePub.getTopic().empty()) - { - globalPosePub = nh.advertise("global_pose", 1); - ROS_INFO("Global pose will be published."); - } - } - - if(odom.data().gps().stamp() > 0.0) - { - if(gpsFixPub.getTopic().empty()) - { - gpsFixPub = nh.advertise("gps/fix", 1); - ROS_INFO("GPS will be published."); - } - } - - // publish transforms first - if(publishTf) - { - rtabmap::Transform localTransform; - if(odom.data().cameraModels().size() == 1) - { - localTransform = odom.data().cameraModels()[0].localTransform(); - } - else if(odom.data().stereoCameraModels().size() == 1) - { - localTransform = odom.data().stereoCameraModels()[0].left().localTransform(); - } - if(!localTransform.isNull()) - { - geometry_msgs::TransformStamped baseToCamera; - baseToCamera.child_frame_id = cameraFrameId; - baseToCamera.header.frame_id = frameId; - baseToCamera.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); - tfBroadcaster.sendTransform(baseToCamera); - } - - if(!odom.pose().isNull()) - { - geometry_msgs::TransformStamped odomToBase; - odomToBase.child_frame_id = frameId; - odomToBase.header.frame_id = odomFrameId; - odomToBase.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); - tfBroadcaster.sendTransform(odomToBase); - } - - if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty()) - { - geometry_msgs::TransformStamped baseToLaserScan; - baseToLaserScan.child_frame_id = scanFrameId; - baseToLaserScan.header.frame_id = frameId; - baseToLaserScan.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); - tfBroadcaster.sendTransform(baseToLaserScan); - } - } - if(!odom.pose().isNull()) - { - if(odometryPub.getTopic().empty()) odometryPub = nh.advertise("odom", 1); - - if(odometryPub.getNumSubscribers()) - { - nav_msgs::Odometry odomMsg; - odomMsg.child_frame_id = frameId; - odomMsg.header.frame_id = odomFrameId; - odomMsg.header.stamp = time; - rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); - UASSERT(odomMsg.pose.covariance.size() == 36 && - odom.covariance().total() == 36 && - odom.covariance().type() == CV_64FC1); - memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); - odometryPub.publish(odomMsg); - } - } - - // Publish async topics first (so that they can catched by rtabmap before the image topics) - if(globalPosePub.getNumSubscribers() > 0 && - !odom.data().globalPose().isNull() && - odom.data().globalPoseCovariance().cols==6 && - odom.data().globalPoseCovariance().rows==6) - { - geometry_msgs::PoseWithCovarianceStamped msg; - rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); - memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); - msg.header.frame_id = frameId; - msg.header.stamp = time; - globalPosePub.publish(msg); - } - - if(odom.data().gps().stamp() > 0.0) - { - sensor_msgs::NavSatFix msg; - msg.longitude = odom.data().gps().longitude(); - msg.latitude = odom.data().gps().latitude(); - msg.altitude = odom.data().gps().altitude(); - msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); - msg.header.frame_id = frameId; - msg.header.stamp.fromSec(odom.data().gps().stamp()); - gpsFixPub.publish(msg); - } - - if(type >= 0) - { - if(rgbCamInfoPub.getNumSubscribers() && type == 0) - { - rgbCamInfoPub.publish(camInfoA); - } - if(leftCamInfoPub.getNumSubscribers() && type == 1) - { - leftCamInfoPub.publish(camInfoA); - } - if(depthCamInfoPub.getNumSubscribers() && type == 0) - { - depthCamInfoPub.publish(camInfoB); - } - if(rightCamInfoPub.getNumSubscribers() && type == 1) - { - rightCamInfoPub.publish(camInfoB); - } - } - - if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers()) - { - cv_bridge::CvImage img; - if(odom.data().imageRaw().channels() == 1) - { - img.encoding = sensor_msgs::image_encodings::MONO8; - } - else - { - img.encoding = sensor_msgs::image_encodings::BGR8; - } - img.image = odom.data().imageRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - if(imagePub.getNumSubscribers()) - { - imagePub.publish(imageRosMsg); - } - if(rgbPub.getNumSubscribers() && type == 0) - { - rgbPub.publish(imageRosMsg); - } - if(leftPub.getNumSubscribers() && type == 1) - { - leftPub.publish(imageRosMsg); - leftCamInfoPub.publish(camInfoA); - } - } - - if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0) - { - cv_bridge::CvImage img; - if(odom.data().depthRaw().type() == CV_32FC1) - { - img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - } - else - { - img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - } - img.image = odom.data().depthRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - depthPub.publish(imageRosMsg); - depthCamInfoPub.publish(camInfoB); - } - - if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1) - { - cv_bridge::CvImage img; - img.encoding = sensor_msgs::image_encodings::MONO8; - img.image = odom.data().rightRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - rightPub.publish(imageRosMsg); - rightCamInfoPub.publish(camInfoB); - } - - if(!odom.data().laserScanRaw().isEmpty()) - { - if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d()) - { - //inspired from pointcloud_to_laserscan package - sensor_msgs::LaserScan msg; - msg.header.frame_id = scanFrameId; - msg.header.stamp = time; - - msg.angle_min = scanAngleMin; - msg.angle_max = scanAngleMax; - msg.angle_increment = scanAngleIncrement; - msg.time_increment = 0.0; - msg.scan_time = 0; - msg.range_min = scanRangeMin; - msg.range_max = scanRangeMax; - if(odom.data().laserScanRaw().angleIncrement() > 0.0f) - { - msg.angle_min = odom.data().laserScanRaw().angleMin(); - msg.angle_max = odom.data().laserScanRaw().angleMax(); - msg.angle_increment = odom.data().laserScanRaw().angleIncrement(); - msg.range_min = odom.data().laserScanRaw().rangeMin(); - msg.range_max = odom.data().laserScanRaw().rangeMax(); - } - - uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); - msg.ranges.assign(rangesSize, 0.0); - - const cv::Mat & scan = odom.data().laserScanRaw().data(); - for (int i=0; i(0,i); - double range = hypot(ptr[0], ptr[1]); - if (range >= msg.range_min && range <=msg.range_max) - { - double angle = atan2(ptr[1], ptr[0]); - if (angle >= msg.angle_min && angle <= msg.angle_max) - { - int index = (angle - msg.angle_min) / msg.angle_increment; - if (index>=0 && index= 7 && // including null str ending - odom.data().userDataRaw().rows == 1 && - memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) - { - //GOAL format detected, remove it from the user data and send it as goal event - std::string goalStr = (const char *)odom.data().userDataRaw().data; - if(!goalStr.empty()) - { - std::list strs = uSplit(goalStr, ':'); - if(strs.size() == 2) - { - int goalId = atoi(strs.rbegin()->c_str()); - - if(goalId > 0) - { - ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId); - rtabmap_msgs::SetGoal setGoalSrv; - setGoalSrv.request.node_id = goalId; - setGoalSrv.request.node_label = ""; - if(!ros::service::call("set_goal", setGoalSrv)) - { - ROS_ERROR("Can't call \"set_goal\" service"); - } - } - } - } - } - - ros::spinOnce(); - - while(ros::ok()) + while(rclcpp::ok()) { #ifndef _WIN32 if (spacehit()) { - paused = !paused; - if(paused) + node->setPaused(!node->isPaused()); + if(node->isPaused()) { - ROS_INFO("paused!"); + RCLCPP_INFO(node->get_logger(), "paused!"); } else { - ROS_INFO("resumed!"); + RCLCPP_INFO(node->get_logger(), "resumed!"); } } #endif - if(!paused) + if(!node->isPaused()) { break; } - uSleep(100); - ros::spinOnce(); + pauseRate.sleep(); + executor.spin_some(); } - - timer.restart(); - cameraInfo = rtabmap::CameraInfo(); - data = reader.takeImage(&cameraInfo); - odomInfo.reg.covariance = cameraInfo.odomCovariance; - odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo); - acquisitionTime = timer.ticks(); } - + rclcpp::shutdown(); return 0; } diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 24a41da4..827d43c1 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -69,6 +69,7 @@ MapsManager::MapsManager() : mapCacheCleanup_(true), alwaysUpdateMap_(false), scanEmptyRayTracing_(true), + localMapsCacheLoadedOnInit_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), occupancyGrid_(new OccupancyGrid(&localMaps_)), @@ -96,6 +97,7 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) alwaysUpdateMap_ = node.declare_parameter("map_always_update", rclcpp::ParameterValue(alwaysUpdateMap_)).get(); scanEmptyRayTracing_ = node.declare_parameter("map_empty_ray_tracing", rclcpp::ParameterValue(scanEmptyRayTracing_)).get(); + localMapsCacheLoadedOnInit_ = node.declare_parameter("map_cache_loaded_on_init", rclcpp::ParameterValue(localMapsCacheLoadedOnInit_)).get(); cloudOutputVoxelized_ = node.declare_parameter("cloud_output_voxelized", rclcpp::ParameterValue(cloudOutputVoxelized_)).get(); cloudSubtractFiltering_ = node.declare_parameter("cloud_subtract_filtering", rclcpp::ParameterValue(cloudSubtractFiltering_)).get(); cloudSubtractFilteringMinNeighbors_ = node.declare_parameter("cloud_subtract_filtering_min_neighbors", rclcpp::ParameterValue(cloudSubtractFilteringMinNeighbors_)).get(); @@ -111,6 +113,7 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false"); + RCLCPP_INFO(node.get_logger(), "%s(maps): map_cache_loaded_on_init = %s", name.c_str(), localMapsCacheLoadedOnInit_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); @@ -272,7 +275,9 @@ void MapsManager::set2DMap( { occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses); //update cache in case the map should be updated - if(memory) + if(memory && + uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0 && // versions 0.11.10+ have local grids saved in db + localMapsCacheLoadedOnInit_) { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { @@ -475,32 +480,55 @@ std::map MapsManager::updateMapCaches( filteredPoses.erase(0); } + const std::map emptyNodes; + const std::map * addedNodes = &emptyNodes; + bool fullUpdateNeeded = true; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + if(updateGrid) { + fullUpdateNeeded = occupancyGrid_->fullUpdateNeeded(filteredPoses); + addedNodes = &occupancyGrid_->addedNodes(); + } +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + if(updateOctomap) { + fullUpdateNeeded = fullUpdateNeeded || octomap_->fullUpdateNeeded(filteredPoses); + if(octomap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &octomap_->addedNodes(); + } + } +#endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation) { + fullUpdateNeeded = fullUpdateNeeded || elevationMap_->fullUpdateNeeded(filteredPoses); + if(elevationMap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &elevationMap_->addedNodes(); + } + } +#endif + if(fullUpdateNeeded) { + UINFO("Full occupancy grid map update needed"); + } + else { + UDEBUG("Full occupancy grid map update not needed"); + } +#endif + bool longUpdate = false; UTimer longUpdateTimer; - if(filteredPoses.size() > 20) + if(fullUpdateNeeded && filteredPoses.size() > 20 && localMaps_.size() < 5) { - if(updateGridCache && localMaps_.size() < 5) - { - UWARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); - longUpdate = true; - } -#ifdef RTABMAP_OCTOMAP - if(updateOctomap && octomap_->addedNodes().size() < 5) - { - UWARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); - longUpdate = true; - } -#endif + UWARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); + longUpdate = true; } bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; - for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || ( + (fullUpdateNeeded || addedNodes->find(iter->first) == addedNodes->end()) && + !uContains(localMaps_.localGrids(), iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -849,11 +877,19 @@ void MapsManager::publishMaps( if(graphGroundOptimized && !tmpGroundPts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpGroundPts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15); +#endif } if(graphObstacleOptimized && !tmpObstaclePts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpObstaclePts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15); +#endif } double indexingTime = t.ticks(); UINFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime); @@ -894,7 +930,11 @@ void MapsManager::publishMaps( } if(!assembledGroundIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { @@ -941,7 +981,11 @@ void MapsManager::publishMaps( } if(!assembledObstacleIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp new file mode 100644 index 00000000..a78ca5fa --- /dev/null +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -0,0 +1,809 @@ +/* +Copyright (c) 2010-2025, 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +namespace rtabmap_util +{ + +DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : + rclcpp::Node("db_player", options), + paused_(false), + frameId_("base_link"), + odomFrameId_("odom"), + cameraFrameId_("camera_optical_link"), + scanFrameId_("base_laser_link"), + gtFrameId_("world"), + gtBaseFrameId_("base_link_gt"), + imuFrameId_("imu_link"), + qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT) +{ + //ULogger::setType(ULogger::kTypeConsole); + //ULogger::setLevel(ULogger::kDebug); + //ULogger::setEventLevel(ULogger::kWarning); + + //parse input arguments + bool publishClock = false; + publishClock = this->declare_parameter("publish_clock", publishClock); + std::vector tmpList = get_node_options().arguments(); + std::vector argList; + for(unsigned int i=0; ideclare_parameter("frame_id", frameId_); + odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_); + cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_); + scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_); + gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_); + gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_); + imuFrameId_ = this->declare_parameter("imu_frame_id", imuFrameId_); + rate = this->declare_parameter("rate", rate); // Ratio of the database stamps + databasePath = this->declare_parameter("database", databasePath); + publishTf = this->declare_parameter("publish_tf", publishTf); + ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom); + startId = this->declare_parameter("start_id", startId); + qos_ = this->declare_parameter("qos", qos_); + qosCameraInfo_ = this->declare_parameter("qos_camera_info", qos_); + qosOdom_ = this->declare_parameter("qos_odom", qos_); + qosScan_ = this->declare_parameter("qos_scan", qos_); + qosScanCloud_ = this->declare_parameter("qos_scan_cloud", qos_); + qosGlobalPose_ = this->declare_parameter("qos_global_pose", qos_); + qosGps_ = this->declare_parameter("qos_gps", qos_); + qosImu_ = this->declare_parameter("qos_imu", qos_); + + // A general 360 lidar with 0.5 deg increment + scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); + scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI); + scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0); + scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0); + scanRangeMax_ = this->declare_parameter("scan_range_max", 60); + + RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str()); + RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "imu_frame_id = %s", imuFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate); + RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false"); + RCLCPP_INFO(get_logger(), "start_id = %d", startId); + RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false"); + RCLCPP_INFO(get_logger(), "qos = %d", qos_); + RCLCPP_INFO(get_logger(), " qos_camera_info = %d", qosCameraInfo_); + RCLCPP_INFO(get_logger(), " qos_odom = %d", qosOdom_); + RCLCPP_INFO(get_logger(), " qos_scan = %d", qosScan_); + RCLCPP_INFO(get_logger(), " qos_scan_cloud = %d", qosScanCloud_); + RCLCPP_INFO(get_logger(), " qos_global_pose = %d", qosGlobalPose_); + RCLCPP_INFO(get_logger(), " qos_gps = %d", qosGps_); + RCLCPP_INFO(get_logger(), " qos_imu = %d", qosImu_); + RCLCPP_INFO(get_logger(), " qos_env_sensor = %d", qosEnvSensor_); + + if(databasePath.empty()) + { + RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); + exit(-1); + } + + databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); + if(databasePath.size() && databasePath.at(0) != '/') + { + databasePath = UDirectory::currentDir(true) + databasePath; + } + RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str()); + + reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId)); + if(!reader_->init()) + { + RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); + exit(-1); + } + + const std::string servicePrefix = get_name() + std::string("/"); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + if(publishTf) { + tfBroadcaster_ = std::make_shared(this); + } + + if(publishClock) + { + clockPub_ = this->create_publisher("/clock", 1); + } +} + +DbPlayer::~DbPlayer(){} + +void DbPlayer::pauseCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + if(paused_) + { + RCLCPP_WARN(get_logger(), "Already paused!"); + } + else + { + paused_ = true; + RCLCPP_INFO(get_logger(), "paused!"); + } +} +void DbPlayer::resumeCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + if(!paused_) + { + RCLCPP_WARN(get_logger(), "Already running!"); + } + else + { + paused_ = false; + RCLCPP_INFO(get_logger(), "resumed!"); + } +} + +void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom) +{ + if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) + { + if(odom.data().cameraModels().size() > 1) + { + if(rgbdImagePubs_.empty()) { + for(size_t i=0;icreate_publisher(uFormat("rgbd_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_))); + RCLCPP_INFO(get_logger(), "RGB-D image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name()); + } + } + else { + UASSERT_MSG(rgbdImagePubs_.size() == odom.data().cameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().cameraModels().size()).c_str()); + } + } + else + { + if(rgbPub_.getTopic().empty()) { +#ifdef PRE_ROS_LYRICAL + rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); +#else + rgbPub_ = image_transport::create_publisher(*this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)); +#endif + RCLCPP_INFO(get_logger(), "Gray/RGB image \"%s\" will be published.", rgbPub_.getTopic().c_str()); + } + if(!rgbInfoPub_.get()) { + rgbInfoPub_ = this->create_publisher("rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Gray/RGB calibration \"%s\" will be published.", rgbInfoPub_->get_topic_name()); + } + if(depthPub_.getTopic().empty()) { +#ifdef PRE_ROS_LYRICAL + depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); +#else + depthPub_ = image_transport::create_publisher(*this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)); +#endif + RCLCPP_INFO(get_logger(), "Depth image \"%s\" will be published.", depthPub_.getTopic().c_str()); + } + if(!depthInfoPub_.get()) { + depthInfoPub_ = this->create_publisher("depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Depth calibration \"%s\" will be published.", depthInfoPub_->get_topic_name()); + } + } + } + else if(!odom.data().rightRaw().empty() && (odom.data().rightRaw().type() == CV_8U || odom.data().rightRaw().type() == CV_8UC3)) + { + if(odom.data().stereoCameraModels().size() > 1) + { + if(rgbdImagePubs_.empty()) { + for(size_t i=0;icreate_publisher(uFormat("stereo_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_))); + RCLCPP_INFO(get_logger(), "Stereo image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name()); + } + } + else { + UASSERT_MSG(rgbdImagePubs_.size() == odom.data().stereoCameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().stereoCameraModels().size()).c_str()); + } + } + else + { + if(leftPub_.getTopic().empty()) { +#ifdef PRE_ROS_LYRICAL + leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); +#else + leftPub_ = image_transport::create_publisher(*this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)); +#endif + RCLCPP_INFO(get_logger(), "Left image \"%s\" will be published.", leftPub_.getTopic().c_str()); + } + if(!leftInfoPub_.get()) { + leftInfoPub_ = this->create_publisher("left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Left calibration \"%s\" will be published.", leftInfoPub_->get_topic_name()); + } + if(rightPub_.getTopic().empty()) { +#ifdef PRE_ROS_LYRICAL + rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); +#else + rightPub_ = image_transport::create_publisher(*this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)); +#endif + RCLCPP_INFO(get_logger(), "Right image \"%s\" will be published.", rightPub_.getTopic().c_str()); + } + if(!rightInfoPub_.get()) { + rightInfoPub_ = this->create_publisher("right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Right calibration \"%s\" will be published.", rightInfoPub_->get_topic_name()); + } + } + + } + else if(imagePub_.getTopic().empty()) + { +#ifdef PRE_ROS_LYRICAL + imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); +#else + imagePub_ = image_transport::create_publisher(*this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)); +#endif + RCLCPP_INFO(get_logger(), "Image \"%s\" without calibration will be published.", imagePub_.getTopic().c_str()); + } + + if(!odom.data().laserScanRaw().isEmpty()) + { + if(!scanPub_.get() && odom.data().laserScanRaw().is2d()) + { + scanPub_ = this->create_publisher("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_)); + if(odom.data().laserScanRaw().angleIncrement() > 0.0f) + { + RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published.", scanPub_->get_topic_name()); + } + else + { + RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published with those parameters:", scanPub_->get_topic_name()); + RCLCPP_INFO(get_logger(), " scan_angle_min=%f", scanAngleMin_); + RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_); + RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_); + RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_); + RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_); + } + } + else if(!scanCloudPub_.get()) + { + scanCloudPub_ = this->create_publisher("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScanCloud_)); + RCLCPP_INFO(get_logger(), "PointCloud2 \"%s\" will be published.", scanCloudPub_->get_topic_name()); + } + } + + if(!odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + if(!globalPosePub_.get()) + { + globalPosePub_ = this->create_publisher("global_pose", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGlobalPose_)); + RCLCPP_INFO(get_logger(), "Global pose \"%s\" will be published.", globalPosePub_->get_topic_name()); + } + } + + if(!gpsFixPub_.get() && odom.data().gps().stamp() > 0.0) + { + gpsFixPub_ = this->create_publisher("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGps_)); + RCLCPP_INFO(get_logger(), "GPS \"%s\" will be published.", gpsFixPub_->get_topic_name()); + } + + if(!envSensorPub_.get() && !odom.data().envSensors().empty()) + { + envSensorPub_ = this->create_publisher("env_sensor", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosEnvSensor_)); + RCLCPP_INFO(get_logger(), "Env sensor \"%s\" will be published.", envSensorPub_->get_topic_name()); + } + + if(!odometryPub_.get() && !odom.pose().isNull()) + { + odometryPub_ = this->create_publisher("odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom_)); + RCLCPP_INFO(get_logger(), "Odometry \"%s\" will be published.", odometryPub_->get_topic_name()); + } + if(!imuPub_.get() && !odom.data().imu().empty()) + { + imuPub_ = this->create_publisher("imu", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosImu_)); + RCLCPP_INFO(get_logger(), "IMU \"%s\" will be published.", imuPub_->get_topic_name()); + } +} + +bool DbPlayer::cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage) +{ + cv_bridge::CvImage img; + if(image.type() == CV_32FC1) + { + img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + else if(image.type() == CV_16UC1) + { + img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + else if(image.type() == CV_8UC1) + { + img.encoding = sensor_msgs::image_encodings::MONO8; + } + else if(image.type() == CV_8UC3) + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + else { + RCLCPP_ERROR(get_logger(), "Unsupported image format: cv type = %d", image.type()); + return false; + } + img.image = image; + img.toImageMsg(rosImage); + return true; +} + +bool DbPlayer::publishNextFrame() +{ + rtabmap::SensorCaptureInfo cameraInfo; + rtabmap::SensorData data = reader_->takeImage(&cameraInfo); + rtabmap::OdometryInfo odomInfo; + odomInfo.reg.covariance = cameraInfo.odomCovariance; + rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); + if(!odom.data().id()) + { + return false; + } + + RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id()); + + rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp()); + + /////////////////////// + // Initialize publishers based on data in the database + /////////////////////// + initializePublishers(odom); // called everytime in case some data like global pose, imu, odometry was not available at the beggining. + + + /////////////////////// + // Publish topics + /////////////////////// + + if(clockPub_.get()) + { + rosgraph_msgs::msg::Clock msg; + msg.clock = time; + clockPub_->publish(msg); + } + + // publish transforms first + if(tfBroadcaster_.get()) + { + std::vector transforms; + const std::vector * models = &odom.data().cameraModels(); + std::vector stereoModels; + bool stereo = false; + if(odom.data().stereoCameraModels().size()) + { + for(const auto & cam: odom.data().stereoCameraModels()) { + stereoModels.push_back(cam.left()); + stereoModels.push_back(cam.right()); + } + models = &stereoModels; + stereo = true; + } + int index = 0; + static bool firstTimeCamMsg = true; + for(const auto & cam: *models) { + rtabmap::Transform localTransform = cam.localTransform(); + if(!localTransform.isNull()) { + geometry_msgs::msg::TransformStamped baseToCamera; + baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (((stereo && models->size()>2) || (!stereo && models->size()>1))?uNumber2Str(index/(stereo?2:1)):""); + baseToCamera.header.frame_id = frameId_; + baseToCamera.header.stamp = time; + if(cam.Tx() != 0) { + localTransform *= rtabmap::Transform(-cam.Tx()/cam.fx(), 0, 0); + } + rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); + transforms.push_back(baseToCamera); + if(firstTimeCamMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToCamera.header.frame_id.c_str(), baseToCamera.child_frame_id.c_str()); + } + } + ++index; + } + firstTimeCamMsg = firstTimeCamMsg && models->empty()?true:false; + + if(!odom.pose().isNull()) + { + geometry_msgs::msg::TransformStamped odomToBase; + odomToBase.child_frame_id = frameId_; + odomToBase.header.frame_id = odomFrameId_; + odomToBase.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); + transforms.push_back(odomToBase); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", odomToBase.header.frame_id.c_str(), odomToBase.child_frame_id.c_str()); + } + firstTimeMsg = false; + + } + + if(scanPub_.get() || scanCloudPub_.get()) + { + geometry_msgs::msg::TransformStamped baseToLaserScan; + baseToLaserScan.child_frame_id = scanFrameId_; + baseToLaserScan.header.frame_id = frameId_; + baseToLaserScan.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); + transforms.push_back(baseToLaserScan); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToLaserScan.header.frame_id.c_str(), baseToLaserScan.child_frame_id.c_str()); + } + firstTimeMsg = false; + } + + if(!odom.data().groundTruth().isNull()) { + geometry_msgs::msg::TransformStamped worldToBase; + worldToBase.child_frame_id = gtBaseFrameId_; + worldToBase.header.frame_id = gtFrameId_; + worldToBase.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); + transforms.push_back(worldToBase); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", worldToBase.header.frame_id.c_str(), worldToBase.child_frame_id.c_str()); + } + firstTimeMsg = false; + } + + if(!odom.data().imu().empty()) { + geometry_msgs::msg::TransformStamped baseToImu; + baseToImu.child_frame_id = imuFrameId_; + baseToImu.header.frame_id = frameId_; + baseToImu.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform); + transforms.push_back(baseToImu); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToImu.header.frame_id.c_str(), baseToImu.child_frame_id.c_str()); + } + firstTimeMsg = false; + } + tfBroadcaster_->sendTransform(transforms); + } + + if( odometryPub_.get() && + !odom.pose().isNull() && + odometryPub_->get_subscription_count()) + { + nav_msgs::msg::Odometry odomMsg; + odomMsg.child_frame_id = frameId_; + odomMsg.header.frame_id = odomFrameId_; + odomMsg.header.stamp = time; + rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); + UASSERT(odomMsg.pose.covariance.size() == 36 && + odom.covariance().total() == 36 && + odom.covariance().type() == CV_64FC1); + memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); + odometryPub_->publish(odomMsg); + } + + // Publish async topics first (so that they can catched by rtabmap before the image topics) + if( globalPosePub_.get() && + globalPosePub_->get_subscription_count() > 0 && + !odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + geometry_msgs::msg::PoseWithCovarianceStamped msg; + rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); + memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); + msg.header.frame_id = frameId_; + msg.header.stamp = time; + globalPosePub_->publish(msg); + } + + if( gpsFixPub_.get() && + gpsFixPub_->get_subscription_count() > 0 && + odom.data().gps().stamp() > 0.0) + { + sensor_msgs::msg::NavSatFix msg; + msg.longitude = odom.data().gps().longitude(); + msg.latitude = odom.data().gps().latitude(); + msg.altitude = odom.data().gps().altitude(); + msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); + msg.header.frame_id = frameId_; + msg.header.stamp = rtabmap_conversions::timestampToROS(odom.data().gps().stamp()); + gpsFixPub_->publish(msg); + } + + if( envSensorPub_.get() && + envSensorPub_->get_subscription_count() > 0 && + !odom.data().envSensors().empty()) + { + rtabmap_msgs::msg::EnvSensor msg; + for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter) + { + rtabmap_msgs::msg::EnvSensor msg; + rtabmap_conversions::envSensorToROS(iter->second, msg); + msg.header.frame_id = frameId_; + if(iter->second.stamp() == 0.0) { + msg.header.stamp = rtabmap_conversions::timestampToROS(data.stamp()); + } + envSensorPub_->publish(msg); + } + } + + if( imuPub_.get() && + imuPub_->get_subscription_count() > 0 && + !odom.data().imu().empty()) + { + sensor_msgs::msg::Imu msg; + rtabmap_conversions::imuToROS(odom.data().imu(), msg); + msg.header.frame_id = imuFrameId_; + msg.header.stamp = time; + imuPub_->publish(msg); + } + + // single camera + if(imagePub_.getNumSubscribers() || (odom.data().cameraModels().size() <= 1 && odom.data().stereoCameraModels().size() <= 1)) + { + if(!odom.data().imageRaw().empty() && + ((imagePub_.getNumSubscribers()) || + (rgbPub_.getNumSubscribers()) || + (leftPub_.getNumSubscribers()))) + { + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().imageRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + if(imagePub_.getNumSubscribers()) + { + imagePub_.publish(imageRosMsg); + } + if(rgbPub_.getNumSubscribers()) + { + rgbPub_.publish(imageRosMsg); + UASSERT(odom.data().cameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info); + info.header = imageRosMsg.header; + rgbInfoPub_->publish(info); + } + if(leftPub_.getNumSubscribers()) + { + imageRosMsg.header.frame_id = "left_" + cameraFrameId_; + leftPub_.publish(imageRosMsg); + UASSERT(odom.data().stereoCameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].left(), info); + info.header = imageRosMsg.header; + leftInfoPub_->publish(info); + } + } + } + + if(!odom.data().depthRaw().empty() && depthPub_.getNumSubscribers()) + { + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().depthRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + depthPub_.publish(imageRosMsg); + + UASSERT(odom.data().cameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + // We assume depth is registered with the RGB camera, so they share same calibration and TF frame + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info); + info.header = imageRosMsg.header; + depthInfoPub_->publish(info); + } + } + + if(!odom.data().rightRaw().empty() && rightPub_.getNumSubscribers()) + { + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().rightRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = "right_" + cameraFrameId_; + imageRosMsg.header.stamp = time; + + rightPub_.publish(imageRosMsg); + + UASSERT(odom.data().stereoCameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].right(), info); + info.header = imageRosMsg.header; + rightInfoPub_->publish(info); + } + } + } + + // Multi-cameras + if(!odom.data().imageRaw().empty()) + { + std::vector rgbdImages; + if(odom.data().cameraModels().size() > 1) + { + UASSERT(odom.data().cameraModels().size() == rgbdImagePubs_.size()); + int subRgbImageWidth = odom.data().imageRaw().cols / odom.data().cameraModels().size(); + int subDepthImageWidth = odom.data().depthRaw().cols / odom.data().cameraModels().size(); + for(size_t i=0; ipublish(msg); + } + } + else if(odom.data().stereoCameraModels().size() > 1) + { + UASSERT(odom.data().stereoCameraModels().size() == rgbdImagePubs_.size()); + int subImageWidth = odom.data().imageRaw().cols / odom.data().stereoCameraModels().size(); + UASSERT(odom.data().imageRaw().cols == odom.data().rightRaw().cols); + for(size_t i=0; ipublish(msg); + } + } + } + + if(!odom.data().laserScanRaw().isEmpty()) + { + if(scanPub_.get() && + scanPub_->get_subscription_count() && + odom.data().laserScanRaw().is2d()) + { + //inspired from pointcloud_to_laserscan package + sensor_msgs::msg::LaserScan msg; + msg.header.frame_id = scanFrameId_; + msg.header.stamp = time; + + msg.angle_min = scanAngleMin_; + msg.angle_max = scanAngleMax_; + msg.angle_increment = scanAngleIncrement_; + msg.time_increment = 0.0; + msg.scan_time = 0; + msg.range_min = scanRangeMin_; + msg.range_max = scanRangeMax_; + if(odom.data().laserScanRaw().angleIncrement() > 0.0f) + { + msg.angle_min = odom.data().laserScanRaw().angleMin(); + msg.angle_max = odom.data().laserScanRaw().angleMax(); + msg.angle_increment = odom.data().laserScanRaw().angleIncrement(); + msg.range_min = odom.data().laserScanRaw().rangeMin(); + msg.range_max = odom.data().laserScanRaw().rangeMax(); + } + + int rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); + msg.ranges.assign(rangesSize, 0.0); + + const cv::Mat & scan = odom.data().laserScanRaw().data(); + for (int i=0; i(0,i); + double range = hypot(ptr[0], ptr[1]); + if (range >= msg.range_min && range <=msg.range_max) + { + double angle = atan2(ptr[1], ptr[0]); + if (angle >= msg.angle_min && angle <= msg.angle_max) + { + int index = (angle - msg.angle_min) / msg.angle_increment; + if (index>=0 && indexpublish(msg); + } + else if(scanCloudPub_.get() && + scanCloudPub_->get_subscription_count()) + { + sensor_msgs::msg::PointCloud2 msg; + pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg); + msg.header.frame_id = scanFrameId_; + msg.header.stamp = time; + scanCloudPub_->publish(msg); + } + } + return true; +} + +} + +#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::DbPlayer) diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index d5b896b6..e537b313 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -45,8 +45,13 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); +#ifdef PRE_ROS_LYRICAL 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()); + pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + pub32f_ = image_transport::create_publisher(*this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); + pub16u_ = image_transport::create_publisher(*this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); +#endif 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/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index 2ebd00a7..d15e189b 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -46,6 +46,16 @@ LidarDeskewing::~LidarDeskewing() void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { + if(scanSyncDiagnostic_.get() == 0) { + scanSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + scanSyncDiagnostic_->init(subScan_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subScan_->get_topic_name())); + } + scanSyncDiagnostic_->tickInput(msg->header.stamp); // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform( msg->header.frame_id, @@ -75,10 +85,23 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); scanOutDeskewed.header.frame_id = msg->header.frame_id; pubScan_->publish(scanOutDeskewed); + + scanSyncDiagnostic_->tickOutput(msg->header.stamp); } void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + if(cloudSyncDiagnostic_.get() == 0) { + cloudSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + cloudSyncDiagnostic_->init(subCloud_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subCloud_->get_topic_name())); + } + cloudSyncDiagnostic_->tickInput(msg->header.stamp); + sensor_msgs::msg::PointCloud2 msgDeskewed; if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_)) { @@ -91,6 +114,7 @@ void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSha RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!"); pubCloud_->publish(*msg); } + cloudSyncDiagnostic_->tickOutput(msg->header.stamp); } } diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index ff81dd54..81302ac1 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -390,8 +390,8 @@ void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::Con #else pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp); #endif - //Make sure row_step is the sum of both - assembledTmp->row_step = assembled->row_step + (*iter)->row_step; + // Make sure row_step is updated + assembledTmp->row_step = assembledTmp->point_step * assembledTmp->width; assembled = assembledTmp; } } diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index f66f4663..98532d7e 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -97,6 +97,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); roiStr = this->declare_parameter("roi_ratios", roiStr); + this->declare_parameter("depth_transport", std::string("raw")); //parse roi (region of interest) roiRatios_.resize(4, 0); @@ -156,8 +157,14 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : 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(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints hints(this, "raw", "depth_transport"); + imageDepthSub_.subscribe(this, depthTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + image_transport::TransportHints hints(*this, "raw", "depth_transport"); + imageDepthSub_.subscribe(*this, depthTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoSub_.subscribe(this, "depth/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); disparitySub_.subscribe(this, "disparity/image", RCLCPP_QOS(topicQueueSize, qos)); @@ -187,7 +194,13 @@ void PointCloudXYZ::callback( { rclcpp::Time time = now(); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg); + cv_bridge::CvImageConstPtr imageDepthPtr; + try{ + imageDepthPtr = cv_bridge::toCvShare(depthMsg); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting depth image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index 5d33ffba..f4dd9b39 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -102,6 +102,8 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); roiStr = this->declare_parameter("roi_ratios", roiStr); + this->declare_parameter("image_transport", std::string("raw")); + this->declare_parameter("depth_transport", std::string("raw")); //parse roi (region of interest) roiRatios_.resize(4, 0); @@ -184,15 +186,32 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : 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(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + image_transport::TransportHints rgbHints(this); // using "image_transport" parameter + image_transport::TransportHints depthHints(this, "raw", "depth_transport"); + imageSub_.subscribe(this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + image_transport::TransportHints rgbHints(*this); // using "image_transport" parameter + image_transport::TransportHints depthHints(*this, "raw", "depth_transport"); + imageSub_.subscribe(*this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); + imageDepthSub_.subscribe(*this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); imageDisparitySub_.subscribe(this, "disparity", RCLCPP_QOS(topicQueueSize, qos)); - 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()); + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a +#ifdef PRE_ROS_LYRICAL + imageLeft_.subscribe(this, leftTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRight_.subscribe(this, rightTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + imageLeft_.subscribe(*this, leftTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); + imageRight_.subscribe(*this, rightTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos)); +#endif cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); } @@ -233,21 +252,32 @@ void PointCloudXYZRGB::depthCallback( rclcpp::Time time = now(); cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); + try { + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting RGB image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); + cv_bridge::CvImageConstPtr imageDepthPtr; + try { + imageDepthPtr = cv_bridge::toCvShare(imageDepth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting Depth image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo); @@ -311,18 +341,24 @@ void PointCloudXYZRGB::disparityCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); + try { + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting RGB image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + return; } if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && @@ -397,16 +433,21 @@ void PointCloudXYZRGB::stereoCallback( rclcpp::Time time = now(); cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; - if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); + try { + if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); + } + else + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); + } + ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); } - else - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f) { diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 070b456e..30c0cab1 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -107,8 +107,13 @@ 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_); +#ifdef PRE_ROS_LYRICAL 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 + 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 +#else + depthImage16Pub_ = image_transport::create_publisher(*this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); // 16 bits unsigned in mm + depthImage32Pub_ = image_transport::create_publisher(*this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));// 32 bits float in meters +#endif pointCloudTransformedPub_ = create_publisher("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cameraInfo16Pub_ = create_publisher(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); cameraInfo32Pub_ = create_publisher(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index d1b85f97..e8c401e7 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -46,8 +46,15 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - 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()); +#ifdef PRE_ROS_LYRICAL + rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); +#else + rgbPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); + depthPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); +#endif + rgbInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1); + depthInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1); } @@ -73,7 +80,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage); #endif } - rgbPub_.publish(outputImage, outputCameraInfo); + rgbPub_.publish(outputImage); + rgbInfoPub_->publish(outputCameraInfo); } if(depthPub_.getNumSubscribers()) @@ -95,8 +103,24 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage); #endif } - outputImage.header = outputCameraInfo.header = input->header; - depthPub_.publish(outputImage, outputCameraInfo); + if(outputCameraInfo.header.frame_id.empty()) { + if(outputImage.header.frame_id.empty()) { + outputCameraInfo.header = input->header; + } + else { + outputCameraInfo.header = outputImage.header; + } + } + if(outputImage.header.frame_id.empty()) { + if(outputCameraInfo.header.frame_id.empty()) { + outputImage.header = input->header; + } + else { + outputImage.header = outputCameraInfo.header; + } + } + depthPub_.publish(outputImage); + depthInfoPub_->publish(outputCameraInfo); } } diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index a7707f22..add21b02 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -43,6 +43,18 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + geometry_msgs::geometry_msgs + rclcpp::rclcpp + std_msgs::std_msgs + std_srvs::std_srvs + nav_msgs::nav_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_sync::rtabmap_sync + tf2::tf2 + rtabmap::gui +) +SET(AmentLibraries cv_bridge geometry_msgs rclcpp @@ -52,6 +64,7 @@ SET(Libraries rtabmap_msgs rtabmap_sync tf2 + RTABMap ) ########### @@ -59,7 +72,11 @@ SET(Libraries ########### add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp include/${PROJECT_NAME}/PreferencesDialogROS.h) -ament_target_dependencies(rtabmap_viz ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_viz ${AmentLibraries}) +else() + target_link_libraries(rtabmap_viz PRIVATE ${Libraries}) +endif() SET_TARGET_PROPERTIES( rtabmap_viz PROPERTIES @@ -68,12 +85,27 @@ SET_TARGET_PROPERTIES( AUTORCC ON ) +add_executable(rgbd_image_viewer src/RGBDImageViewerNode.cpp src/rgbd_image_viewer.cpp include/${PROJECT_NAME}/rgbd_image_viewer.hpp) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rgbd_image_viewer ${AmentLibraries}) +else() + target_link_libraries(rgbd_image_viewer PRIVATE ${Libraries}) +endif() +SET_TARGET_PROPERTIES( + rgbd_image_viewer + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + ############# ## Install ## ############# install(TARGETS rtabmap_viz + rgbd_image_viewer DESTINATION lib/${PROJECT_NAME} ) diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index f263871d..e0fce234 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -38,8 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/core/Transform.h" -#include -#include +#include +#include #include #include @@ -89,20 +89,6 @@ private: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); - virtual void commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - 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()); virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -130,9 +116,9 @@ private: private: rtabmap::PreferencesDialog * prefDialog_; rtabmap::MainWindow * mainWindow_; - std::string cameraNodeName_; double lastOdomInfoUpdateTime_; std::string rtabmapNodeName_; + std::string odometryNodeName_; // odometry subscription stuffs std::string frameId_; diff --git a/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp b/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp new file mode 100644 index 00000000..87be0549 --- /dev/null +++ b/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp @@ -0,0 +1,84 @@ +/* +Copyright (c) 2010-2025, 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. +*/ + +#ifndef RGBDIMAGEVIEWER_H_ +#define RGBDIMAGEVIEWER_H_ + +#include +#include +#include +#include "rtabmap_msgs/msg/rgbd_image.hpp" +#include +#include +#include +#include + +namespace rtabmap +{ + class CameraViewer; +} + +class QComboBox; +class QSpinBox; +class QLabel; + +namespace rtabmap_viz { + +class RGBDImageViewer : public QMainWindow, public UEventsSender +{ + Q_OBJECT + +public: + RTABMAP_VIZ_PUBLIC + explicit RGBDImageViewer(std::shared_ptr & node, const rtabmap::ParametersMap & parameters); + virtual ~RGBDImageViewer(); + +private Q_SLOTS: + void updateTopicList(); + void topicSelected(const QString & topicName); + +private: + void callback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg); + +private: + QComboBox * topicComboBox_; + QComboBox * frameComboBox_; + QSpinBox * spinBox_; + QLabel * warningLabel_; + rtabmap::CameraViewer * cameraView_; + rclcpp::Subscription::SharedPtr rgbdImageSub_; + + std::shared_ptr node_; + std::shared_ptr tfBuffer_; + std::shared_ptr tfListener_; + + std::mutex mutex_; +}; + +} + +#endif /* RGBDIMAGEVIEWER_H_ */ diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index c57bf971..420b9335 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -2,7 +2,7 @@ rtabmap_viz - 0.22.1 + 0.23.7 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/src/GuiNode.cpp b/rtabmap_viz/src/GuiNode.cpp index c01125a3..88895ede 100644 --- a/rtabmap_viz/src/GuiNode.cpp +++ b/rtabmap_viz/src/GuiNode.cpp @@ -37,7 +37,7 @@ QApplication * app = 0; void my_handler(int){ UINFO("rtabmap_viz: ctrl-c catched! Exiting Qt app..."); - app->exit(-1); + app->exit(0); } int main(int argc, char** argv) @@ -87,8 +87,12 @@ int main(int argc, char** argv) r = app->exec();// MUST be called by the Main Thread RCLCPP_INFO(node->get_logger(), "rtabmap_viz stopping spinner..."); - rclcpp::shutdown(); - execution_thread.join(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + if (execution_thread.joinable()) { + execution_thread.join(); + } RCLCPP_INFO(node->get_logger(), "rtabmap_viz: All done! Closing..."); } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 44ac1c97..9190794b 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -65,9 +65,9 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : Node("rtabmap_viz", options), rtabmap_sync::CommonDataSubscriber(*this, true), mainWindow_(0), - cameraNodeName_(""), lastOdomInfoUpdateTime_(0), rtabmapNodeName_("rtabmap"), + odometryNodeName_("rgbd_odometry"), frameId_("base_link"), odomFrameId_(""), waitForTransform_(0.2), // 200 ms @@ -93,7 +93,10 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : configFile.replace('~', QDir::homePath()); - rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); + rtabmapNodeName_ = this->declare_parameter("rtabmap_node_name", rtabmapNodeName_); + odometryNodeName_ = this->declare_parameter("odometry_node_name", odometryNodeName_); + RCLCPP_INFO(get_logger(), "%s: rtabmap_node_name = %s", get_name(), rtabmapNodeName_.c_str()); + RCLCPP_INFO(get_logger(), "%s: odometry_node_name = %s", get_name(), odometryNodeName_.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Using configuration from \"%s\"", configFile.toStdString().c_str()); uSleep(500); @@ -114,9 +117,17 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_); - cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process subscribeInfoOnly = this->declare_parameter("subscribe_info_only", subscribeInfoOnly); initCachePath = this->declare_parameter("init_cache_path", initCachePath); + + RCLCPP_INFO(get_logger(), "%s: frame_id = \"%s\"", get_name(), frameId_.c_str()); + RCLCPP_INFO(get_logger(), "%s: odom_frame_id = \"%s\"", get_name(), odomFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "%s: wait_for_transform = %f", get_name(), waitForTransform_); + RCLCPP_INFO(get_logger(), "%s: odom_sensor_sync = %s", get_name(), odomSensorSync_?"true":"false"); + RCLCPP_INFO(get_logger(), "%s: max_odom_update_rate = %f", get_name(), maxOdomUpdateRate_); + RCLCPP_INFO(get_logger(), "%s: subscribe_info_only = %s", get_name(), subscribeInfoOnly?"true":"false"); + RCLCPP_INFO(get_logger(), "%s: init_cache_path = \"%s\"", get_name(), initCachePath.c_str()); + if(initCachePath.size()) { initCachePath = uReplaceChar(initCachePath, '~', UDirectory::homeDir()); @@ -203,7 +214,11 @@ void GuiWrapper::infoMapCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } @@ -236,7 +251,11 @@ void GuiWrapper::infoCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } void GuiWrapper::goalPathCallback( @@ -319,7 +338,7 @@ bool GuiWrapper::callMapDataService(const std::string & name, bool global, bool } else { - RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available.", name.c_str()); + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", name.c_str()); } return false; } @@ -348,7 +367,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) RCLCPP_INFO(this->get_logger(), "Parameters updated"); auto client = std::make_shared(this, rtabmapNodeName_); if (!client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(this->get_logger(), "Can't call rtabmap parameters service, is the node running?"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s\" parameters service, is the node running? If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } else { @@ -366,28 +385,18 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) { if(!callEmptyService(rtabmapNodeName_+"/reset")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause) { - // Pause the camera if the rtabmap/camera node is used - if(!cameraNodeName_.empty()) - { - std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str()); - if(system(str.c_str()) !=0) - { - RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str()); - } - } - - // Pause visual_odometry - callEmptyService("pause_odom"); + // Pause visual_odometry (can fail silently) + callEmptyService(odometryNodeName_ + "/pause_odom"); // Pause rtabmap if(!callEmptyService(rtabmapNodeName_+"/pause")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/pause\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume) @@ -395,27 +404,17 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) // Resume rtabmap if(!callEmptyService(rtabmapNodeName_+"/resume")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/resume\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } - // Pause visual_odometry - callEmptyService("resume_odom"); - - // Resume the camera if the rtabmap/camera node is used - if(!cameraNodeName_.empty()) - { - std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str()); - if(system(str.c_str()) !=0) - { - RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str()); - } - } + // Pause visual_odometry (can fail silently) + callEmptyService(odometryNodeName_ + "/resume_odom"); } else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap) { if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/trigger_new_map\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap) @@ -458,14 +457,14 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"set_goal\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/set_goal\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal) { if(!callEmptyService(rtabmapNodeName_+"/cancel_goal")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/cancel_goal\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdLabel) @@ -484,7 +483,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/set_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel) @@ -500,7 +499,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/remove_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdRepublishData) @@ -517,9 +516,9 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else if(anEvent->getClassName().compare("OdometryResetEvent") == 0) { - if(!callEmptyService("reset_odom")) + if(!callEmptyService(odometryNodeName_ + "/reset_odom")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset_odom\" service (will only work with rtabmap's odometry nodes, you can remap the node name with \"odometry_node_name\" parameter)", odometryNodeName_.c_str()); } } return false; @@ -582,6 +581,11 @@ void GuiWrapper::commonMultiCameraCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -739,197 +743,6 @@ void GuiWrapper::commonMultiCameraCallback( QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); } -void GuiWrapper::commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr &, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector &, - const std::vector &, - const std::vector &, - const cv::Mat &) -{ - 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()) - { - frameId = odomMsg->child_frame_id; - } - else - { - RCLCPP_WARN(get_logger(), "Received odom topic with child_frame_id not set! Using \"%s\" as base frame.", frameId_.c_str()); - } - } - else - { - if(!scan2dMsg.ranges.empty()) - { - odomHeader = scan2dMsg.header; - } - else if(!scan3dMsg.data.empty()) - { - odomHeader = scan3dMsg.header; - } - else - { - 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; - } - } - - 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) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); - } - } - else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36) - { - if(odomInfoMsg->covariance[0] != 0 && - odomInfoMsg->covariance[7] != 0 && - odomInfoMsg->covariance[14] != 0 && - odomInfoMsg->covariance[21] != 0 && - odomInfoMsg->covariance[28] != 0 && - odomInfoMsg->covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone(); - } - } - if(odomHeader.frame_id.empty()) - { - RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?"); - return; - } - - cv::Mat left; - cv::Mat right; - LaserScan scan; - rtabmap::StereoCameraModel stereoModel; - rtabmap::OdometryInfo info; - bool ignoreData = false; - - // limit update rate - if(maxOdomUpdateRate_<=0.0 || - (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ && - !mainWindow_->isProcessingOdometry() && - !mainWindow_->isProcessingStatistics())) - { - lastOdomInfoUpdateTime_ = UTimer::now(); - - ParametersMap allParameters = prefDialog_->getAllParameters(); - bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified(); - Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified); - - if(!rtabmap_conversions::convertStereoMsg( - leftImageMsg, - rightImageMsg, - leftCamInfoMsg, - rightCamInfoMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - left, - right, - stereoModel, - *tfBuffer_, - waitForTransform_, - imagesAlreadyRectified)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap_viz update..."); - return; - } - - if(!scan2dMsg.ranges.empty()) - { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - - if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - ignoreData = false; - } - else if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); - ignoreData = true; - } - else - { - // don't update GUI odom stuff if we don't use visual odometry - return; - } - - info.reg.covariance = covariance; - rtabmap::OdometryEvent odomEvent( - rtabmap::SensorData( - scan, - left, - right, - stereoModel, - 0, - rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomT, - info); - - QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); -} - void GuiWrapper::commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr &, @@ -970,6 +783,12 @@ void GuiWrapper::commonLaserScanCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -1186,6 +1005,12 @@ void GuiWrapper::commonSensorDataCallback( odomHeader = sensorDataMsg->header; odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index cecd841e..dea76bf1 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -149,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 \"%s\" parameters service, is the node running? If necessary, you can remap the expected rtabmap node name with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } int readCount = 0; if(client->service_is_ready()) diff --git a/rtabmap_viz/src/RGBDImageViewerNode.cpp b/rtabmap_viz/src/RGBDImageViewerNode.cpp new file mode 100644 index 00000000..f277ac85 --- /dev/null +++ b/rtabmap_viz/src/RGBDImageViewerNode.cpp @@ -0,0 +1,83 @@ +/* +Copyright (c) 2010-2025, 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 "rtabmap_viz/rgbd_image_viewer.hpp" +#include "rtabmap/utilite/ULogger.h" + +#include +#include +#include +#include + +QApplication * app = 0; + +void my_handler(int){ + app->exit(-1); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + app = new QApplication(argc, argv); + app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); + + int r; + { + auto node = std::make_shared("rgbd_image_viewer"); + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv, true); + rtabmap_viz::RGBDImageViewer viewer(node, parameters); + viewer.show(); + + // Catch ctrl-c to close the gui + // (Place this after QApplication's constructor) + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = my_handler; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); + + // Here start the ROS events loop + rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread + executor.add_node(node); + auto spin_executor = [&executor]() { + executor.spin(); + }; + + // Launch executer + std::thread execution_thread(spin_executor); + + // Now wait for application to finish + r = app->exec();// MUST be called by the Main Thread + + rclcpp::shutdown(); + execution_thread.join(); + } + delete app; + + return r; +} diff --git a/rtabmap_viz/src/rgbd_image_viewer.cpp b/rtabmap_viz/src/rgbd_image_viewer.cpp new file mode 100644 index 00000000..d9fd1953 --- /dev/null +++ b/rtabmap_viz/src/rgbd_image_viewer.cpp @@ -0,0 +1,171 @@ +/* +Copyright (c) 2010-2025, 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 "rtabmap_viz/rgbd_image_viewer.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rtabmap_viz { + +RGBDImageViewer::RGBDImageViewer(std::shared_ptr & node, const rtabmap::ParametersMap & parameters) : + node_(node) +{ + this->setWindowTitle("rgbd_image_viewer"); + + topicComboBox_ = new QComboBox(this); + topicComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents); + topicComboBox_->setToolTip("Available rtabmap_msgs::RGBDImage topics"); + frameComboBox_ = new QComboBox(this); + frameComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents); + frameComboBox_->setToolTip("Base frame of the point cloud"); + spinBox_ = new QSpinBox(this); + spinBox_->setMinimum(0); + spinBox_->setMaximum(1000); + spinBox_->setValue(10); + spinBox_->setSuffix(" ms"); + spinBox_->setToolTip("Maximum time to wait for TF to transform in base frame (0 means latest available)"); + warningLabel_ = new QLabel(this); + warningLabel_->setStyleSheet("QLabel { color : red; }"); + cameraView_ = new rtabmap::CameraViewer(this, parameters); + cameraView_->registerToEventsManager(); + QPushButton * refreshButton = new QPushButton(this); + refreshButton->setIcon(style()->standardIcon(QStyle::SP_BrowserReload)); + refreshButton->setToolTip("Refresh topics and frames"); + + connect(topicComboBox_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(topicSelected(const QString &))); + connect(cameraView_, SIGNAL(finished(int)), this, SLOT(close())); + connect(refreshButton, SIGNAL(clicked()), this, SLOT(updateTopicList())); + + QWidget *centralWidget = new QWidget(this); + QVBoxLayout *layout = new QVBoxLayout(centralWidget); + + QHBoxLayout *hlayout = new QHBoxLayout(); + hlayout->addWidget(topicComboBox_); + hlayout->addWidget(frameComboBox_); + hlayout->addWidget(spinBox_); + hlayout->addWidget(refreshButton); + hlayout->addWidget(warningLabel_); + hlayout->addStretch(); + + layout->addLayout(hlayout); + layout->addWidget(cameraView_); + + this->setCentralWidget(centralWidget); + + tfBuffer_ = std::make_shared(node_->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + updateTopicList(); +} + +RGBDImageViewer::~RGBDImageViewer() +{ +} + +void RGBDImageViewer::updateTopicList() { + std::map> topicNames = node_->get_topic_names_and_types(); + topicComboBox_->clear(); + for(auto topic: topicNames) { + for(auto type: topic.second) { + if(type == "rtabmap_msgs/msg/RGBDImage") { + topicComboBox_->addItem(topic.first.c_str()); + } + } + } + std::vector frames = tfBuffer_->getAllFrameNames(); + frameComboBox_->clear(); + frameComboBox_->addItem(""); + for(auto & frame: frames) { + frameComboBox_->addItem(frame.c_str()); + } +} + +void RGBDImageViewer::topicSelected(const QString & topicName) { + rgbdImageSub_.reset(); + if(!topicName.isEmpty()) { + rgbdImageSub_ = node_->create_subscription(topicName.toStdString(), rclcpp::QoS(1), std::bind(&RGBDImageViewer::callback, this, std::placeholders::_1)); + } +} + +void RGBDImageViewer::callback( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg) +{ + bool warned = false; + rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(msg); + if(!frameComboBox_->currentText().isEmpty() && (!data.cameraModels().empty() || !data.stereoCameraModels().empty())) { + rtabmap::Transform localTransform; + if(frameComboBox_->currentText().compare("") == 0) { + localTransform = rtabmap::CameraModel::opticalRotation(); + } + else { + localTransform = rtabmap_conversions::getTransform( + frameComboBox_->currentText().toStdString(), + msg->header.frame_id, + msg->header.stamp, + *tfBuffer_, + double(spinBox_->value()) / 1000.0); + } + if(localTransform.isNull()) + { + QString log = QString("Could not get TF between \"%1\" and \"%2\" frames for stamp %3 after waiting %4 ms.") + .arg(frameComboBox_->currentText()) + .arg(msg->header.frame_id.c_str()) + .arg(QString::number(rclcpp::Time(msg->header.stamp).seconds(), 'f', 3)) + .arg(spinBox_->value()); + warningLabel_->setToolTip(log); + QMetaObject::invokeMethod(warningLabel_, "setText", Q_ARG(QString, log)); + warned = true; + } + + if(!data.cameraModels().empty()) { + rtabmap::CameraModel model = data.cameraModels()[0]; + model.setLocalTransform(localTransform); + data.setCameraModel(model); + } + else { + rtabmap::StereoCameraModel model = data.stereoCameraModels()[0]; + model.setLocalTransform(localTransform); + data.setStereoCameraModel(model); + } + } + if(!warned) { + warningLabel_->setToolTip(""); + QMetaObject::invokeMethod(warningLabel_, "clear"); + } + + this->post(new rtabmap::SensorEvent(data)); +} + +}