diff --git a/.github/ISSUE_TEMPLATE b/.github/ISSUE_TEMPLATE deleted file mode 100644 index 196642772..000000000 --- a/.github/ISSUE_TEMPLATE +++ /dev/null @@ -1,25 +0,0 @@ -Thank you for filing an issue! - -It would help us tremendously if you run through the following checklist. This -ensures that we have the most information to quickly understand, analyze, -reproduce and eventually resolve your issue. - -Please - -1. run `rosbag_validate` which does some checks on your sensor data. This - tool often finds issues that can explain poor performance and must be fixed - at recording time. Please post the full output of the tool into a - GitHub Gist at https://gist.github.com/, then link it in the issue even if - it does not report anything. You can run the tool like this: - - rosrun cartographer_ros cartographer_rosbag_validate -bag_filename - -2. post a link to a Git repository containing a branch of - `cartographer_ros` containing all the configuration, launch, and URDF files - required to reproduce your issue. -3. post a link to a bag file we can use to reproduce your issue. Put it on - Google Drive, Dropbox, any webserver or wherever it is publicly - downloadable. -4. remove this boilerplate text before submitting your issue. - -We will likely be unable to help you without all of these points addressed. diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 000000000..b1d8c54bb --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,19 @@ +--- +name: Bug report +about: Report a bug that you've found in this repository or in a release. + +--- + +Please provide information for your bug report: + +- if you're using cartographer_ros from source, provide the Git commit hash + (via `git log -1 --format="%H"`): + +- if you're using a release package, provide the version + (e.g. via `apt show `): + +- provide all information that is needed to analyze and reproduce the bug + (logs, commands that have been used, ...) + +PLEASE NOTE: we don't support custom forks or extensions. +If you need tuning advice, you can open a tuning issue instead. diff --git a/.github/ISSUE_TEMPLATE/general-issue.md b/.github/ISSUE_TEMPLATE/general-issue.md new file mode 100644 index 000000000..24180eb67 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/general-issue.md @@ -0,0 +1,5 @@ +--- +name: General issue +about: Use this issue for general questions. + +--- diff --git a/.github/ISSUE_TEMPLATE/tuning-advice.md b/.github/ISSUE_TEMPLATE/tuning-advice.md new file mode 100644 index 000000000..48442f65e --- /dev/null +++ b/.github/ISSUE_TEMPLATE/tuning-advice.md @@ -0,0 +1,30 @@ +--- +name: Help request for tuning +about: Describe tuning problems with your custom Cartographer setup to request help from the community. + +--- + +To improve the chances that other Cartographer users can help you, +here are some guidelines for describing your tuning problem: + +1. check the tuning guide on the documentation page and previous issues - + some problems might have been solved already by other people. + +2. run `rosbag_validate` which does some checks on your sensor data. This + tool often finds issues that can explain poor performance and must be fixed + at recording time. Please post the full output of the tool into a + GitHub Gist at https://gist.github.com/, then link it in the issue even if + it does not report anything. You can run the tool like this: + + rosrun cartographer_ros cartographer_rosbag_validate -bag_filename + +3. post a link to a Git repository containing a branch of + `cartographer_ros` containing all the configuration, launch, and URDF files + required to reproduce your issue. +4. post a link to a bag file we can use to reproduce your issue. Put it on + Google Drive, Dropbox, any webserver or wherever it is publicly + downloadable. +5. remove this boilerplate text before submitting your issue. + +PLEASE NOTE: tuning issues are not necessarily handled by the maintainers and +can be closed after a period of inactivity. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 000000000..bf6c78eae --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,2 @@ +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md). diff --git a/.travis.yml b/.travis.yml index a8e9a4291..41286c677 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,10 +22,9 @@ cache: - /home/travis/docker/ env: - - ROS_RELEASE=indigo DOCKER_CACHE_FILE=/home/travis/docker/indigo-cache.tar.gz - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz - - ROS_RELEASE=lunar DOCKER_CACHE_FILE=/home/travis/docker/lunar-cache.tar.gz - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz + - ROS_RELEASE=noetic DOCKER_CACHE_FILE=/home/travis/docker/noetic-cache.tar.gz before_install: # $GITHUB_TOKEN must be a valid GitHub access token without access rights (https://github.com/settings/tokens). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2827b7d3f..42d286974 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,27 +1,51 @@ -Want to contribute? Great! First, read this page (including the small print at the end). +Want to contribute? Great! First, read this page. ### Before you contribute -Before we can use your code, you must sign the -[Google Individual Contributor License Agreement] -(https://cla.developers.google.com/about/google-individual) -(CLA), which you can do online. The CLA is necessary mainly because you own the -copyright to your changes, even after your contribution becomes part of our -codebase, so we need your permission to use and distribute your code. We also -need to be sure of various other things—for instance that you'll tell us if you -know that your code infringes on other people's patents. You don't have to sign -the CLA until after you've submitted your code for review and a member has -approved it, but you must do it before we can put your code into our codebase. -Before you start working on a larger contribution, you should get in touch with -us first through the issue tracker with your idea so that we can help out and -possibly guide you. Coordinating up front makes it much easier to avoid -frustration later on. + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. ### Code reviews -All submissions, including submissions by project members, require review. We -use Github pull requests for this purpose. - -### The small print -Contributions made by corporations are covered by a different agreement than -the one above, the -[Software Grant and Corporate Contributor License Agreement] -(https://cla.developers.google.com/about/google-corporate). + +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. + +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run tests as appropriate, e.g. `docker build . -t cartographer:noetic -f Dockerfile.noetic`. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. diff --git a/Dockerfile.indigo b/Dockerfile.indigo deleted file mode 100644 index 2ea9ed60e..000000000 --- a/Dockerfile.indigo +++ /dev/null @@ -1,76 +0,0 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - -FROM ros:indigo - -ARG CARTOGRAPHER_VERSION=master - -# We require a GitHub access token to be passed. -ARG github_token - -# First, we invalidate the entire cache if googlecartographer/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - -# Build, install, and test all packages individually to allow caching. The -# ordering of these steps must match the topological package ordering as -# determined by Catkin. -COPY scripts/install.sh cartographer_ros/scripts/ -COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ - -COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs - -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - -COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros - -COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ - cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz - -COPY scripts/ros_entrypoint.sh / -# A BTRFS bug may prevent us from cleaning up these directories. -# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory -RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index d7a26c373..7e2bb8d5f 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:kinetic +FROM osrf/ros:kinetic-desktop ARG CARTOGRAPHER_VERSION=master @@ -20,12 +20,12 @@ ARG CARTOGRAPHER_VERSION=master ARG github_token # Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y sudo -# First, we invalidate the entire cache if googlecartographer/cartographer has +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. @@ -39,9 +39,10 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh -# Install proto3. +# Install Abseil and proto3. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh # Build, install, and test all packages individually to allow caching. The @@ -50,17 +51,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ @@ -74,6 +73,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.melodic b/Dockerfile.melodic index 3091e771f..35e8399be 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:melodic +FROM osrf/ros:melodic-desktop ARG CARTOGRAPHER_VERSION=master @@ -20,12 +20,12 @@ ARG CARTOGRAPHER_VERSION=master ARG github_token # Bionic's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y sudo -# First, we invalidate the entire cache if googlecartographer/cartographer has +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. @@ -39,10 +39,10 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh +# Install Abseil. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh # Build, install, and test all packages individually to allow caching. The # ordering of these steps must match the topological package ordering as @@ -50,17 +50,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ @@ -74,6 +72,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.lunar b/Dockerfile.noetic similarity index 83% rename from Dockerfile.lunar rename to Dockerfile.noetic index f5af22ad1..7168d10e6 100644 --- a/Dockerfile.lunar +++ b/Dockerfile.noetic @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# Copyright 2020 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,20 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:lunar +FROM osrf/ros:noetic-desktop ARG CARTOGRAPHER_VERSION=master # We require a GitHub access token to be passed. ARG github_token -# Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +# Prevent any interaction required by apt-get. +# https://stackoverflow.com/questions/22466255 +ARG DEBIAN_FRONTEND=noninteractive -# First, we invalidate the entire cache if googlecartographer/cartographer has +# ROS Noetic's base image doesn't ship with sudo and git. +RUN apt-get update && apt-get install -y sudo git + +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. @@ -39,10 +43,10 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh +# Install Abseil. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh # Build, install, and test all packages individually to allow caching. The # ordering of these steps must match the topological package ordering as @@ -50,17 +54,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ @@ -74,6 +76,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true diff --git a/README.rst b/README.rst index 9032f42ff..fb72eb6e2 100644 --- a/README.rst +++ b/README.rst @@ -25,7 +25,7 @@ Purpose and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor configurations. This project provides Cartographer's ROS integration. -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping Getting started @@ -35,7 +35,7 @@ Getting started * You can ask a question by `creating an issue`_. .. _our Read the Docs site: https://google-cartographer-ros.readthedocs.io -.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question Contributing ============ @@ -43,12 +43,12 @@ Contributing You can find information about contributing to Cartographer's ROS integration at `our Contribution page`_. -.. _our Contribution page: https://github.com/googlecartographer/cartographer_ros/blob/master/CONTRIBUTING.md +.. _our Contribution page: https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md -.. |build| image:: https://travis-ci.org/googlecartographer/cartographer_ros.svg?branch=master +.. |build| image:: https://travis-ci.org/cartographer-project/cartographer_ros.svg?branch=master :alt: Build Status :scale: 100% - :target: https://travis-ci.org/googlecartographer/cartographer_ros + :target: https://travis-ci.org/cartographer-project/cartographer_ros .. |docs| image:: https://readthedocs.org/projects/google-cartographer-ros/badge/?version=latest :alt: Documentation Status :scale: 100% @@ -56,5 +56,5 @@ at `our Contribution page`_. .. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg :alt: Apache 2 license. :scale: 100% - :target: https://github.com/googlecartographer/cartographer_ros/blob/master/LICENSE + :target: https://github.com/cartographer-project/cartographer_ros/blob/master/LICENSE diff --git a/azure-pipelines.yml b/azure-pipelines.yml new file mode 100644 index 000000000..a778158c9 --- /dev/null +++ b/azure-pipelines.yml @@ -0,0 +1,70 @@ +# Copyright 2018 The Cartographer Authors +# +# 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. + +jobs: +- job: Build + pool: + vmImage: 'vs2017-win2016' + timeoutInMinutes: 360 + steps: + - script: | + choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1 + rem Azure VM runs out of space on C:, so use D: for ros and rosdeps + mkdir D:\opt && mklink /J C:\opt D:\opt + choco upgrade %ROS_METAPACKAGE% -y + choco upgrade ros-melodic-perception -y + robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL + git clone https://github.com/cartographer-project/cartographer src\cartographer + call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y + env: + ROS_METAPACKAGE: 'ros-melodic-desktop' + displayName: Install prerequisites + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release + displayName: Build + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + cd build_isolated\cartographer\install && ctest --no-compress-output -T Test + displayName: Run cartographer tests + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test + displayName: Build and run cartographer_ros tests + condition: always() + + - script: | + call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat" + call "C:\opt\ros\melodic\x64\setup.bat" + call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat + python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros + displayName: Convert tests to jUnit + condition: always() + + - task: PublishTestResults@2 + displayName: Publish test results + inputs: + testRunner: 'jUnit' + testResultsFiles: '**\jUnit.xml' + searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing' + condition: always() diff --git a/cartographer_ros.rosinstall b/cartographer_ros.rosinstall index 89dae9bd4..430ce8497 100644 --- a/cartographer_ros.rosinstall +++ b/cartographer_ros.rosinstall @@ -1,3 +1,2 @@ -- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'} -- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'} -- git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'} +- git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: 'master'} +- git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: 'master'} diff --git a/cartographer_ros/CHANGELOG.rst b/cartographer_ros/CHANGELOG.rst index 608bbd3ba..b81daa209 100644 --- a/cartographer_ros/CHANGELOG.rst +++ b/cartographer_ros/CHANGELOG.rst @@ -2,30 +2,6 @@ Changelog for package cartographer_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.0.9004 (2021-10-28) ---------------------- -* add pbstream_to_ros_map_node (`#54 `_) -* Use the non-deprecated tf2_eigen.hpp header. (`#55 `_) -* Contributors: Chris Lalancette, Liangqian - -1.0.9003 (2021-01-25) ---------------------- -* Switch to using a C-style string for RCLCPP macros. -* Cleanup the CMakeLists.txt. -* Contributors: Chris Lalancette - -1.0.9002 (2020-10-30) ---------------------- -* Handle multiple ROS messages with the same timestamp (`#46 `_) -* Contributors: Michel Hidalgo - -1.0.9001 (2020-02-06) ---------------------- -* Init rclcpp first (`#41 `_) -* Strip ROS args before passing to GFlags (`#40 `_) -* Fix argument intiialization to allow node to take in params (`#39 `_) -* Contributors: Emerson Knapp, Shane Loretz - 1.0.0 (2018-06-01) ---------------------- * https://github.com/googlecartographer/cartographer_ros/compare/0.3.0...1.0.0 diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 82e4e8e29..1185320eb 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -1,4 +1,5 @@ # Copyright 2016 The Cartographer Authors +# Copyright 2022 Wyca Robotics (for the ROS2 conversion) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,23 +19,16 @@ project(cartographer_ros) find_package(ament_cmake REQUIRED) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +# Default to C++17 +set(CMAKE_CXX_STANDARD 17) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -find_package(cartographer REQUIRED) -include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") -set(BUILD_SHARED_LIBS OFF) -option(BUILD_GRPC "build features that require Cartographer gRPC support" false) - -google_initialize_cartographer_project() -# google_enable_testing() +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +find_package(cartographer REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -47,50 +41,56 @@ find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_msgs REQUIRED) find_package(visualization_msgs REQUIRED) - -find_package(LuaGoogle REQUIRED) +find_package(rosbag2_compression REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_storage REQUIRED) +find_package(absl REQUIRED) +find_package(Lua REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(Eigen3 REQUIRED) +find_package(urdf REQUIRED) find_package(urdfdom_headers REQUIRED) -if(DEFINED urdfdom_headers_VERSION) - if(${urdfdom_headers_VERSION} GREATER 0.4.1) - add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS) - endif() -endif() - include_directories( include - "." + ${LUA_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} ) -# # Override Catkin's GTest configuration to use GMock. -# set(GTEST_FOUND TRUE) -# set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS}) -# set(GTEST_LIBRARIES ${GMOCK_LIBRARIES}) - -# file(GLOB_RECURSE ALL_SRCS "*.cc" "*.h") -# file(GLOB_RECURSE ALL_TESTS "*_test.cc") -# file(GLOB_RECURSE ALL_EXECUTABLES "*_main.cc") -# list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS}) -# list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES}) - -set(ALL_SRCS - "cartographer_ros/map_builder_bridge.cc" - "cartographer_ros/msg_conversion.cc" - "cartographer_ros/node.cc" - "cartographer_ros/node_constants.cc" - "cartographer_ros/node_options.cc" - "cartographer_ros/ros_log_sink.cc" - "cartographer_ros/sensor_bridge.cc" - "cartographer_ros/tf_bridge.cc" - "cartographer_ros/time_conversion.cc" - "cartographer_ros/trajectory_options.cc" - "cartographer_ros/submap.cc" - "cartographer_ros/ros_map.cc" -) -add_library(${PROJECT_NAME} ${ALL_SRCS}) -ament_target_dependencies(${PROJECT_NAME} PUBLIC +# Library +add_library(${PROJECT_NAME} + src/assets_writer.cpp + src/assets_writer_main.cpp + src/map_builder_bridge.cpp + src/msg_conversion.cpp + src/node_constants.cpp + src/node.cpp + src/node_main.cpp + src/node_options.cpp + src/occupancy_grid_node_main.cpp + src/offline_node.cpp + src/offline_node_main.cpp + src/pbstream_map_publisher_main.cpp + src/pbstream_to_ros_map_main.cpp + src/playable_bag.cpp + src/rosbag_validate_main.cpp + src/ros_log_sink.cpp + src/ros_map.cpp + src/ros_map_writing_points_processor.cpp + src/sensor_bridge.cpp + src/submap.cpp + src/tf_bridge.cpp + src/time_conversion.cpp + src/trajectory_options.cpp + src/urdf_reader.cpp + src/metrics/family_factory.cpp + src/metrics/internal/family.cpp + src/metrics/internal/histogram.cpp + ) + +set(dependencies + cartographer cartographer_ros_msgs geometry_msgs nav_msgs @@ -103,74 +103,111 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC tf2_msgs tf2_ros visualization_msgs + rosbag2_compression + rosbag2_cpp + rosbag2_storage + urdf + urdfdom ) -add_subdirectory("cartographer_ros") - -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) - -# Lua -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - -# PCL -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) -set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ") -foreach(DEFINITION ${PCL_DEFINITIONS}) - list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX) - if (${DEFINITIONS_INDEX} GREATER -1) - continue() - endif() - set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") -endforeach() - -# Eigen -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${EIGEN3_INCLUDE_DIR}") - -# YAML -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${YAMLCPP_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${YAMLCPP_LIBRARIES}) - -# URDFDOM -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${urdfdom_headers_INCLUDE_DIRS}) - -# Add the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ -) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} + ) +target_link_libraries(${PROJECT_NAME} cartographer ${PCL_LIBRARIES}) -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) +# Executables +add_executable(cartographer_node src/node_main.cpp) +target_link_libraries(cartographer_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_node ${dependencies}) -# install(PROGRAMS scripts/tf_remove_frames.py -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +add_executable(cartographer_occupancy_grid_node src/occupancy_grid_node_main.cpp) +target_link_libraries(cartographer_occupancy_grid_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_occupancy_grid_node ${dependencies}) + +add_executable(cartographer_offline_node src/offline_node_main.cpp) +target_link_libraries(cartographer_offline_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_offline_node ${dependencies}) + +add_executable(cartographer_assets_writer src/assets_writer_main.cpp) +target_link_libraries(cartographer_assets_writer ${PROJECT_NAME}) +ament_target_dependencies(cartographer_assets_writer ${dependencies}) + +add_executable(cartographer_pbstream_map_publisher src/pbstream_map_publisher_main.cpp) +target_link_libraries(cartographer_pbstream_map_publisher ${PROJECT_NAME}) +ament_target_dependencies(cartographer_pbstream_map_publisher ${dependencies}) + +add_executable(cartographer_pbstream_to_ros_map src/pbstream_to_ros_map_main.cpp) +target_link_libraries(cartographer_pbstream_to_ros_map ${PROJECT_NAME}) +ament_target_dependencies(cartographer_pbstream_to_ros_map ${dependencies}) + +add_executable(cartographer_rosbag_validate src/rosbag_validate_main.cpp) +target_link_libraries(cartographer_rosbag_validate ${PROJECT_NAME}) +ament_target_dependencies(cartographer_rosbag_validate ${dependencies}) -install(DIRECTORY configuration_files - DESTINATION share/${PROJECT_NAME}/ -) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -# # Install source headers. -# file(GLOB_RECURSE HDRS "*.h") -# foreach(HDR ${HDRS}) -# file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) -# get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) -# install( -# FILES -# ${HDR} -# DESTINATION -# include/${INSTALL_DIR} -# ) -# endforeach() + RUNTIME DESTINATION bin +) +install(TARGETS + cartographer_node + cartographer_occupancy_grid_node + cartographer_offline_node + cartographer_assets_writer + cartographer_pbstream_map_publisher + cartographer_pbstream_to_ros_map + cartographer_rosbag_validate + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY configuration_files urdf launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) ament_package() + +# Non converted bin: +#google_binary(cartographer_dev_pbstream_trajectories_to_rosbag +# SRCS +# dev/pbstream_trajectories_to_rosbag_main.cc +#) + +#google_binary(cartographer_dev_rosbag_publisher +# SRCS +# dev/rosbag_publisher_main.cc +#) + + +#google_binary(cartographer_dev_trajectory_comparison +# SRCS +# dev/trajectory_comparison_main.cc +#) + + +## TODO(cschuet): Add support for shared library case. +#if (${BUILD_GRPC}) +# google_binary(cartographer_grpc_node +# SRCS +# cartographer_grpc/node_grpc_main.cc +# ) + + +# google_binary(cartographer_grpc_offline_node +# SRCS +# cartographer_grpc/offline_node_grpc_main.cc +# ) + +# install(PROGRAMS +# ../scripts/cartographer_grpc_server.sh +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +#endif() diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt deleted file mode 100644 index b8512c084..000000000 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ /dev/null @@ -1,109 +0,0 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - -add_executable(cartographer_node - node_main.cc) -target_include_directories(cartographer_node SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) -target_link_libraries(cartographer_node ${PROJECT_NAME}) -ament_target_dependencies(cartographer_node - rclcpp -) - -add_executable(occupancy_grid_node - occupancy_grid_node_main.cc) -target_include_directories(occupancy_grid_node SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) -target_link_libraries(occupancy_grid_node ${PROJECT_NAME}) -ament_target_dependencies(occupancy_grid_node - cartographer_ros_msgs - nav_msgs - rclcpp -) - -add_executable(pbstream_to_ros_map_node - pbstream_to_ros_map_main.cc) -target_include_directories(pbstream_to_ros_map_node SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) -target_link_libraries(pbstream_to_ros_map_node ${PROJECT_NAME}) - -install(TARGETS - cartographer_node - occupancy_grid_node - pbstream_to_ros_map_node - DESTINATION lib/${PROJECT_NAME}) - -# google_binary(cartographer_assets_writer -# SRCS -# assets_writer_main.cc -# ros_map_writing_points_processor.h -# ros_map_writing_points_processor.cc -# ) -# -# install(TARGETS cartographer_assets_writer -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# -# google_binary(cartographer_offline_node -# SRCS -# offline_node_main.cc -# ) -# -# install(TARGETS cartographer_offline_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -# -# google_binary(cartographer_start_trajectory -# SRCS -# start_trajectory_main.cc -# ) -# -# install(TARGETS cartographer_start_trajectory -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -# -# google_binary(cartographer_occupancy_grid_node -# SRCS -# occupancy_grid_node_main.cc -# ) -# -# install(TARGETS cartographer_occupancy_grid_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -# -# google_binary(cartographer_rosbag_validate -# SRCS -# rosbag_validate_main.cc -# ) -# -# install(TARGETS cartographer_rosbag_validate -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -# -# google_binary(cartographer_pbstream_to_ros_map -# SRCS -# pbstream_to_ros_map_main.cc -# ) -# -# install(TARGETS cartographer_pbstream_to_ros_map -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc deleted file mode 100644 index 0af416ade..000000000 --- a/cartographer_ros/cartographer_ros/node.cc +++ /dev/null @@ -1,779 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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. - */ - -#include "cartographer_ros/node.h" - -#include -#include -#include -#include - -#include "Eigen/Core" -#include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/make_unique.h" -#include "cartographer/common/port.h" -#include "cartographer/common/time.h" -#include "cartographer/mapping/proto/submap_visualization.pb.h" -#include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer/transform/transform.h" -#include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/sensor_bridge.h" -#include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros/time_conversion.h" -#include "cartographer_ros_msgs/msg/status_code.hpp" -#include "cartographer_ros_msgs/msg/status_response.hpp" -#include "glog/logging.h" -#include - -#include -#include -#include "visualization_msgs/msg/marker_array.hpp" - -namespace cartographer_ros { - -namespace { - -cartographer_ros_msgs::msg::SensorTopics DefaultSensorTopics() { - cartographer_ros_msgs::msg::SensorTopics topics; - topics.laser_scan_topic = kLaserScanTopic; - topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; - topics.point_cloud2_topic = kPointCloud2Topic; - topics.imu_topic = kImuTopic; - topics.odometry_topic = kOdometryTopic; - topics.nav_sat_fix_topic = kNavSatFixTopic; - topics.landmark_topic = kLandmarkTopic; - return topics; -} - -// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and -// calls 'handler' on the 'node' to handle messages. Returns the subscriber. -template -::rclcpp::SubscriptionBase::SharedPtr SubscribeWithHandler( - void (Cartographer::*handler)(int, const std::string&, - const typename MessageType::ConstSharedPtr), - const int trajectory_id, const std::string& topic, - ::rclcpp::Node::SharedPtr node_handle, Cartographer* const node) { - auto last_message_time = std::make_shared( - std::numeric_limits::min(), RCL_ROS_TIME); - return node_handle->create_subscription( - topic, rclcpp::SensorDataQoS(), - [node, handler, trajectory_id, topic, last_message_time](const typename MessageType::ConstSharedPtr msg) { - rclcpp::Time current_message_time = msg->header.stamp; - if (current_message_time > *last_message_time) { - (node->*handler)(trajectory_id, topic, msg); - } - *last_message_time = current_message_time; - }); -} - -} // namespace - -namespace carto = ::cartographer; - -using carto::transform::Rigid3d; - -Cartographer::Cartographer( - const NodeOptions& node_options, - std::unique_ptr map_builder) - : Node("cartographer_node"), - node_options_(node_options) -{ - node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - - constexpr double kTfBufferCacheTimeInSeconds = 10.; - tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster(node_handle_)); - tf_buffer_.reset(new tf2_ros::Buffer(get_clock(), ::tf2::durationFromSec(kTfBufferCacheTimeInSeconds))); - tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); - - map_builder_bridge_.reset(new cartographer_ros::MapBuilderBridge(node_options_, std::move(map_builder), tf_buffer_.get())); - - carto::common::MutexLocker lock(&mutex_); - - submap_list_publisher_ = - this->create_publisher<::cartographer_ros_msgs::msg::SubmapList>( - kSubmapListTopic, 10); - trajectory_node_list_publisher_ = - this->create_publisher<::visualization_msgs::msg::MarkerArray>( - kTrajectoryNodeListTopic, 10); - landmark_poses_list_publisher_ = - this->create_publisher<::visualization_msgs::msg::MarkerArray>( - kLandmarkPosesListTopic, 10); - constraint_list_publisher_ = - this->create_publisher<::visualization_msgs::msg::MarkerArray>( - kConstraintListTopic, 10); - scan_matched_point_cloud_publisher_ = - this->create_publisher( - kScanMatchedPointCloudTopic, 10); - - auto submap_query_callback = - [this](const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -> void - { - HandleSubmapQuery(request_header, request, response); - }; - - submap_query_server_ = create_service( - kSubmapQueryServiceName, submap_query_callback); - - auto start_trajectory_callback = - [this](const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -> void - { - HandleStartTrajectory(request_header, request, response); - }; - - start_trajectory_server_ = create_service( - kStartTrajectoryServiceName, start_trajectory_callback); - - auto finish_trajectory_callback = - [this](const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -> void - { - HandleFinishTrajectory(request_header, request, response); - }; - - finish_trajectory_server_ = create_service( - kFinishTrajectoryServiceName, finish_trajectory_callback); - - auto write_state_callback = - [this](const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -> void - { - HandleWriteState(request_header, request, response); - }; - - write_state_server_ = create_service( - kWriteStateServiceName, write_state_callback); - - submap_list_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(node_options_.submap_publish_period_sec * 1000)), - [this]() { - PublishSubmapList(); - }); - - trajectory_states_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(node_options_.pose_publish_period_sec * 1000)), - [this]() { - PublishTrajectoryStates(); - }); - - trajectory_node_list_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), - [this]() { - PublishTrajectoryNodeList(); - }); - - landmark_pose_list_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), - [this]() { - PublishLandmarkPosesList(); - }); - - constrain_list_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(kConstraintPublishPeriodSec * 1000)), - [this]() { - PublishConstraintList(); - }); -} - -Cartographer::~Cartographer() { FinishAllTrajectories(); } - -::rclcpp::Node::SharedPtr Cartographer::node_handle() { return node_handle_; } - -void Cartographer::HandleSubmapQuery( - const std::shared_ptr request_header, - const std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Response> response) { - - (void)request_header; - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_->HandleSubmapQuery(request, response); -} - -void Cartographer::PublishSubmapList() { - carto::common::MutexLocker lock(&mutex_); - submap_list_publisher_->publish(map_builder_bridge_->GetSubmapList(this->now())); -} - -void Cartographer::AddExtrapolator(const int trajectory_id, - const TrajectoryOptions& options) { - constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms - CHECK(extrapolators_.count(trajectory_id) == 0); - const double gravity_time_constant = - node_options_.map_builder_options.use_trajectory_builder_3d() - ? options.trajectory_builder_options.trajectory_builder_3d_options() - .imu_gravity_time_constant() - : options.trajectory_builder_options.trajectory_builder_2d_options() - .imu_gravity_time_constant(); - extrapolators_.emplace( - std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - gravity_time_constant)); -} - -void Cartographer::AddSensorSamplers(const int trajectory_id, - const TrajectoryOptions& options) { - CHECK(sensor_samplers_.count(trajectory_id) == 0); - sensor_samplers_.emplace( - std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple( - options.rangefinder_sampling_ratio, options.odometry_sampling_ratio, - options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio, - options.landmarks_sampling_ratio)); -} - -void Cartographer::PublishTrajectoryStates() { - carto::common::MutexLocker lock(&mutex_); - for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { - const auto& trajectory_state = entry.second; - - auto& extrapolator = extrapolators_.at(entry.first); - // We only publish a point cloud if it has changed. It is not needed at high - // frequency, and republishing it would be computationally wasteful. - if (trajectory_state.local_slam_data->time != - extrapolator.GetLastPoseTime()) { - if (this->count_subscribers(kScanMatchedPointCloudTopic) > 0) { - // TODO(gaschler): Consider using other message without time - // information. - carto::sensor::TimedPointCloud point_cloud; - point_cloud.reserve(trajectory_state.local_slam_data - ->range_data_in_local.returns.size()); - for (const Eigen::Vector3f point : - trajectory_state.local_slam_data->range_data_in_local.returns) { - Eigen::Vector4f point_time; - point_time << point, 0.f; - point_cloud.push_back(point_time); - } - scan_matched_point_cloud_publisher_->publish(ToPointCloud2Message( - carto::common::ToUniversal(trajectory_state.local_slam_data->time), - node_options_.map_frame, - carto::sensor::TransformTimedPointCloud( - point_cloud, trajectory_state.local_to_map.cast()))); - } - extrapolator.AddPose(trajectory_state.local_slam_data->time, - trajectory_state.local_slam_data->local_pose); - } - - geometry_msgs::msg::TransformStamped stamped_transform; - // If we do not publish a new point cloud, we still allow time of the - // published poses to advance. If we already know a newer pose, we use its - // time instead. Since tf knows how to interpolate, providing newer - // information is better. - const ::cartographer::common::Time now = std::max( - FromRos(this->now()), extrapolator.GetLastExtrapolatedTime()); - stamped_transform.header.stamp = ToRos(now); - - const Rigid3d tracking_to_local = [&] { - if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) { - return carto::transform::Embed3D( - carto::transform::Project2D(extrapolator.ExtrapolatePose(now))); - } - return extrapolator.ExtrapolatePose(now); - }(); - - const Rigid3d tracking_to_map = - trajectory_state.local_to_map * tracking_to_local; - - if (trajectory_state.published_to_tracking != nullptr) { - if (trajectory_state.trajectory_options.provide_odom_frame) { - std::vector stamped_transforms; - - stamped_transform.header.frame_id = node_options_.map_frame; - stamped_transform.child_frame_id = - trajectory_state.trajectory_options.odom_frame; - stamped_transform.transform = - ToGeometryMsgTransform(trajectory_state.local_to_map); - stamped_transforms.push_back(stamped_transform); - - stamped_transform.header.frame_id = - trajectory_state.trajectory_options.odom_frame; - stamped_transform.child_frame_id = - trajectory_state.trajectory_options.published_frame; - stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_local * (*trajectory_state.published_to_tracking)); - stamped_transforms.push_back(stamped_transform); - - tf_broadcaster_->sendTransform(stamped_transforms); - } else { - stamped_transform.header.frame_id = node_options_.map_frame; - stamped_transform.child_frame_id = - trajectory_state.trajectory_options.published_frame; - stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_map * (*trajectory_state.published_to_tracking)); - tf_broadcaster_->sendTransform(stamped_transform); - } - } - } -} - -void Cartographer::PublishTrajectoryNodeList() { - if (this->count_subscribers(kTrajectoryNodeListTopic) > 0) { - carto::common::MutexLocker lock(&mutex_); - trajectory_node_list_publisher_->publish( - map_builder_bridge_->GetTrajectoryNodeList(this->now())); - } -} - -void Cartographer::PublishLandmarkPosesList() { - if (this->count_subscribers(kLandmarkPosesListTopic) > 0) { - carto::common::MutexLocker lock(&mutex_); - constraint_list_publisher_->publish(map_builder_bridge_->GetLandmarkPosesList(this->now())); - } -} - -void Cartographer::PublishConstraintList() { - if (this->count_subscribers(kConstraintListTopic) > 0) { - carto::common::MutexLocker lock(&mutex_); - constraint_list_publisher_->publish(map_builder_bridge_->GetConstraintList(this->now())); - } -} - -std::set -Cartographer::ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics) const { - using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; - using SensorType = SensorId::SensorType; - std::set expected_topics; - // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.laser_scan_topic, options.num_laser_scans)) { - expected_topics.insert(SensorId{SensorType::RANGE, topic}); - } - for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - options.num_multi_echo_laser_scans)) { - expected_topics.insert(SensorId{SensorType::RANGE, topic}); - } - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.point_cloud2_topic, options.num_point_clouds)) { - expected_topics.insert(SensorId{SensorType::RANGE, topic}); - } - // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is - // required. - if (node_options_.map_builder_options.use_trajectory_builder_3d() || - (node_options_.map_builder_options.use_trajectory_builder_2d() && - options.trajectory_builder_options.trajectory_builder_2d_options() - .use_imu_data())) { - expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic}); - } - // Odometry is optional. - if (options.use_odometry) { - expected_topics.insert( - SensorId{SensorType::ODOMETRY, topics.odometry_topic}); - } - // NavSatFix is optional. - if (options.use_nav_sat) { - expected_topics.insert( - SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); - } - // Landmark is optional. - if (options.use_landmarks) { - expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic}); - } - return expected_topics; -} - -int Cartographer::AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics) { - const std::set - expected_sensor_ids = ComputeExpectedSensorIds(options, topics); - const int trajectory_id = - map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); - AddExtrapolator(trajectory_id, options); - AddSensorSamplers(trajectory_id, options); - LaunchSubscribers(options, topics, trajectory_id); - is_active_trajectory_[trajectory_id] = true; - for (const auto& sensor_id : expected_sensor_ids) { - subscribed_topics_.insert(sensor_id.id); - } - return trajectory_id; -} - -void Cartographer::LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics, - const int trajectory_id) { - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.laser_scan_topic, options.num_laser_scans)) { - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Cartographer::HandleLaserScanMessage, trajectory_id, topic, node_handle_, - this), - topic}); - } - - for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - options.num_multi_echo_laser_scans)) { - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Cartographer::HandleMultiEchoLaserScanMessage, trajectory_id, topic, - node_handle_, this), - topic}); - } - - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.point_cloud2_topic, options.num_point_clouds)) { - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Cartographer::HandlePointCloud2Message, trajectory_id, topic, - node_handle_, this), - topic}); - } - - // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is - // required. - if (node_options_.map_builder_options.use_trajectory_builder_3d() || - (node_options_.map_builder_options.use_trajectory_builder_2d() && - options.trajectory_builder_options.trajectory_builder_2d_options() - .use_imu_data())) { - std::string topic = topics.imu_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Cartographer::HandleImuMessage, - trajectory_id, topic, - node_handle_, this), - topic}); - } - - if (options.use_odometry) { - std::string topic = topics.odometry_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Cartographer::HandleOdometryMessage, - trajectory_id, topic, - node_handle_, this), - topic}); - } - - if (options.use_nav_sat) { - std::string topic = topics.nav_sat_fix_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Cartographer::HandleNavSatFixMessage, - trajectory_id, topic, - node_handle_, this), - topic}); - } - - if (options.use_landmarks) { - std::string topic = topics.landmark_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Cartographer::HandleLandmarkMessage, - trajectory_id, topic, - node_handle_, this), - topic}); - } -} - -bool Cartographer::ValidateTrajectoryOptions(const TrajectoryOptions& options) { - if (node_options_.map_builder_options.use_trajectory_builder_2d()) { - return options.trajectory_builder_options - .has_trajectory_builder_2d_options(); - } - if (node_options_.map_builder_options.use_trajectory_builder_3d()) { - return options.trajectory_builder_options - .has_trajectory_builder_3d_options(); - } - return false; -} - -bool Cartographer::ValidateTopicNames( - const ::cartographer_ros_msgs::msg::SensorTopics& topics, - const TrajectoryOptions& options) { - for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) { - const std::string& topic = sensor_id.id; - if (subscribed_topics_.count(topic) > 0) { - LOG(ERROR) << "Topic name [" << topic << "] is already used."; - return false; - } - } - return true; -} - -cartographer_ros_msgs::msg::StatusResponse Cartographer::FinishTrajectoryUnderLock( - const int trajectory_id) { - cartographer_ros_msgs::msg::StatusResponse status_response; - - // First, check if we can actually finish the trajectory. - if (map_builder_bridge_->GetFrozenTrajectoryIds().count(trajectory_id)) { - const std::string error = - "Trajectory " + std::to_string(trajectory_id) + " is frozen."; - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; - status_response.message = error; - return status_response; - } - if (is_active_trajectory_.count(trajectory_id) == 0) { - const std::string error = - "Trajectory " + std::to_string(trajectory_id) + " is not created yet."; - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND; - status_response.message = error; - return status_response; - } - if (!is_active_trajectory_[trajectory_id]) { - const std::string error = "Trajectory " + std::to_string(trajectory_id) + - " has already been finished."; - LOG(ERROR) << error; - status_response.code = - cartographer_ros_msgs::msg::StatusCode::RESOURCE_EXHAUSTED; - status_response.message = error; - return status_response; - } - - // Shutdown the subscribers of this trajectory. - for (auto& entry : subscribers_[trajectory_id]) { - subscribed_topics_.erase(entry.topic); - LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; - } - CHECK_EQ(subscribers_.erase(trajectory_id), 1); - CHECK(is_active_trajectory_.at(trajectory_id)); - map_builder_bridge_->FinishTrajectory(trajectory_id); - is_active_trajectory_[trajectory_id] = false; - const std::string message = - "Finished trajectory " + std::to_string(trajectory_id) + "."; - status_response.code = cartographer_ros_msgs::msg::StatusCode::OK; - status_response.message = message; - return status_response; -} - -void Cartographer::HandleStartTrajectory( - const std::shared_ptr request_header, - const std::shared_ptr<::cartographer_ros_msgs::srv::StartTrajectory::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::StartTrajectory::Response> response) { - - (void)request_header; - carto::common::MutexLocker lock(&mutex_); - TrajectoryOptions options; - if (!FromRosMessage(request->options, &options) || - !ValidateTrajectoryOptions(options)) { - const std::string error = "Invalid trajectory options."; - LOG(ERROR) << error; - response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; - response->status.message = error; - } - else if (!ValidateTopicNames(request->topics, options)) { - const std::string error = "Invalid topics."; - LOG(ERROR) << error; - response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; - response->status.message = error; - } - else { - response->trajectory_id = AddTrajectory(options, request->topics); - response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; - response->status.message = "Success."; - } -} - -void Cartographer::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { - carto::common::MutexLocker lock(&mutex_); - CHECK(ValidateTrajectoryOptions(options)); - AddTrajectory(options, DefaultSensorTopics()); -} - -std::vector< - std::set> -Cartographer::ComputeDefaultSensorIdsForMultipleBags( - const std::vector& bags_options) const { - using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; - std::vector> bags_sensor_ids; - for (size_t i = 0; i < bags_options.size(); ++i) { - std::string prefix; - if (bags_options.size() > 1) { - prefix = "bag_" + std::to_string(i + 1) + "_"; - } - std::set unique_sensor_ids; - for (const auto& sensor_id : - ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) { - unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); - } - bags_sensor_ids.push_back(unique_sensor_ids); - } - return bags_sensor_ids; -} - -int Cartographer::AddOfflineTrajectory( - const std::set& - expected_sensor_ids, - const TrajectoryOptions& options) { - carto::common::MutexLocker lock(&mutex_); - const int trajectory_id = - map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); - AddExtrapolator(trajectory_id, options); - AddSensorSamplers(trajectory_id, options); - is_active_trajectory_[trajectory_id] = true; - return trajectory_id; -} - -void Cartographer::HandleFinishTrajectory( - const std::shared_ptr request_header, - const std::shared_ptr<::cartographer_ros_msgs::srv::FinishTrajectory::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::FinishTrajectory::Response> response) { - - (void)request_header; - carto::common::MutexLocker lock(&mutex_); - response->status = FinishTrajectoryUnderLock(request->trajectory_id); -} - -void Cartographer::HandleWriteState( - const std::shared_ptr request_header, - const std::shared_ptr<::cartographer_ros_msgs::srv::WriteState::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::WriteState::Response> response) { - - (void)request_header; - carto::common::MutexLocker lock(&mutex_); - if (map_builder_bridge_->SerializeState(request->filename)) { - response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; - response->status.message = "State written to '" + request->filename + "'."; - } else { - response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; - response->status.message = "Failed to write '" + request->filename + "'."; - } -} - -void Cartographer::FinishAllTrajectories() { - carto::common::MutexLocker lock(&mutex_); - for (auto& entry : is_active_trajectory_) { - const int trajectory_id = entry.first; - if (entry.second) { - CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, - cartographer_ros_msgs::msg::StatusCode::OK); - } - } -} - -bool Cartographer::FinishTrajectory(const int trajectory_id) { - carto::common::MutexLocker lock(&mutex_); - return FinishTrajectoryUnderLock(trajectory_id).code == - cartographer_ros_msgs::msg::StatusCode::OK; -} - -void Cartographer::RunFinalOptimization() { - { - carto::common::MutexLocker lock(&mutex_); - for (const auto& entry : is_active_trajectory_) { - CHECK(!entry.second); - } - } - // Assuming we are not adding new data anymore, the final optimization - // can be performed without holding the mutex. - map_builder_bridge_->RunFinalOptimization(); -} - -void Cartographer::HandleOdometryMessage(const int trajectory_id, - const std::string& sensor_id, - const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { - return; - } - auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); - auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); - if (odometry_data_ptr != nullptr) { - extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr); - } - sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); -} - -void Cartographer::HandleNavSatFixMessage(const int trajectory_id, - const std::string& sensor_id, - const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { - return; - } - map_builder_bridge_->sensor_bridge(trajectory_id) - ->HandleNavSatFixMessage(sensor_id, msg); -} - -void Cartographer::HandleLandmarkMessage( - const int trajectory_id, const std::string& sensor_id, - const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { - return; - } - map_builder_bridge_->sensor_bridge(trajectory_id) - ->HandleLandmarkMessage(sensor_id, msg); -} - -void Cartographer::HandleImuMessage(const int trajectory_id, - const std::string& sensor_id, - const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { - return; - } - auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); - auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); - if (imu_data_ptr != nullptr) { - extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); - } - sensor_bridge_ptr->HandleImuMessage(sensor_id, msg); -} - -void Cartographer::HandleLaserScanMessage(const int trajectory_id, - const std::string& sensor_id, - const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { - return; - } - map_builder_bridge_->sensor_bridge(trajectory_id) - ->HandleLaserScanMessage(sensor_id, msg); -} - -void Cartographer::HandleMultiEchoLaserScanMessage( - int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { - return; - } - map_builder_bridge_->sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage(sensor_id, msg); -} - -void Cartographer::HandlePointCloud2Message( - const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { - carto::common::MutexLocker lock(&mutex_); - if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { - return; - } - map_builder_bridge_->sensor_bridge(trajectory_id) - ->HandlePointCloud2Message(sensor_id, msg); -} - -void Cartographer::SerializeState(const std::string& filename) { - carto::common::MutexLocker lock(&mutex_); - CHECK(map_builder_bridge_->SerializeState(filename)) - << "Could not write state."; -} - -void Cartographer::LoadState(const std::string& state_filename, - const bool load_frozen_state) { - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_->LoadState(state_filename, load_frozen_state); -} - - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc deleted file mode 100644 index 73becfa37..000000000 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ /dev/null @@ -1,200 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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. - */ - -#include -#include -#include - -#include "Eigen/Core" -#include "Eigen/Geometry" -#include "cairo/cairo.h" -#include "cartographer/common/mutex.h" -#include "cartographer/common/port.h" -#include "cartographer/io/image.h" -#include "cartographer/io/submap_painter.h" -#include "cartographer/mapping/id.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/node_constants.h" -#include "cartographer_ros/ros_log_sink.h" -#include "cartographer_ros/submap.h" -#include "cartographer_ros_msgs/msg/submap_list.hpp" -#include "cartographer_ros_msgs/srv/submap_query.hpp" -#include "gflags/gflags.h" - -#include -#include - -DEFINE_double(resolution, 0.05, - "Resolution of a grid cell in the published occupancy grid."); -DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period."); - -namespace cartographer_ros { -namespace { - -using ::cartographer::io::PaintSubmapSlicesResult; -using ::cartographer::io::SubmapSlice; -using ::cartographer::mapping::SubmapId; - -class OccupancyGridNode : public rclcpp::Node -{ - public: - explicit OccupancyGridNode(const double resolution, const double publish_period_sec) - : Node("cartographer_occupancy_grid_node"), - resolution_(resolution) - { - client_ = this->create_client(kSubmapQueryServiceName); - - occupancy_grid_publisher_ = this->create_publisher<::nav_msgs::msg::OccupancyGrid>( - kOccupancyGridTopic, rclcpp::QoS(10).transient_local()); - - occupancy_grid_publisher_timer_ = this->create_wall_timer( - std::chrono::milliseconds(int(publish_period_sec * 1000)), - [this]() { - DrawAndPublish(); - }); - - auto handleSubmapList = - [this](const typename cartographer_ros_msgs::msg::SubmapList::SharedPtr msg) -> void - { - ::cartographer::common::MutexLocker locker(&mutex_); - - // We do not do any work if nobody listens. - if (this->count_publishers(kSubmapListTopic) == 0){ - RCLCPP_WARN(this->get_logger(), "topic(/submap_list) is not found"); - return; - } - - // Keep track of submap IDs that don't appear in the message anymore. - std::set submap_ids_to_delete; - for (const auto& pair : submap_slices_) { - submap_ids_to_delete.insert(pair.first); - } - - for (const auto& submap_msg : msg->submap) { - const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; - submap_ids_to_delete.erase(id); - SubmapSlice& submap_slice = submap_slices_[id]; - submap_slice.pose = ToRigid3d(submap_msg.pose); - submap_slice.metadata_version = submap_msg.submap_version; - if (submap_slice.surface != nullptr && - submap_slice.version == submap_msg.submap_version) { - continue; - } - - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return; - } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); - } - - auto srv_request = std::make_shared(); - srv_request->trajectory_id = id.trajectory_id; - srv_request->submap_index = id.submap_index; - - using ServiceResponseFuture = - ::rclcpp::Client::SharedFuture; - auto response_received_callback = [&submap_slice](ServiceResponseFuture future) { - auto fetched_textures = cartographer_ros::FetchSubmapTextures(future.get()); - if (fetched_textures == nullptr) { - return; - } - CHECK(!fetched_textures->textures.empty()); - submap_slice.version = fetched_textures->version; - - // We use the first texture only. By convention this is the highest - // resolution texture and that is the one we want to use to construct the - // map for ROS. - const auto fetched_texture = fetched_textures->textures.begin(); - submap_slice.width = fetched_texture->width; - submap_slice.height = fetched_texture->height; - submap_slice.slice_pose = fetched_texture->slice_pose; - submap_slice.resolution = fetched_texture->resolution; - submap_slice.cairo_data.clear(); - submap_slice.surface = ::cartographer::io::DrawTexture( - fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, - fetched_texture->width, fetched_texture->height, - &submap_slice.cairo_data); - }; - - auto future_result = client_->async_send_request(srv_request, response_received_callback); - } - - // Delete all submaps that didn't appear in the message. - for (const auto& id : submap_ids_to_delete) { - submap_slices_.erase(id); - } - - last_timestamp_ = msg->header.stamp; - last_frame_id_ = msg->header.frame_id; - }; - - submap_list_subscriber_ = create_subscription( - kSubmapListTopic, rclcpp::QoS(10), handleSubmapList); - } - - void DrawAndPublish(void) - { - if (submap_slices_.empty() || last_frame_id_.empty()) - { - RCLCPP_WARN(this->get_logger(), "submap_slices and last_frame_id is empty"); - return; - } - - ::cartographer::common::MutexLocker locker(&mutex_); - auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); - std::unique_ptr msg_ptr = CreateOccupancyGridMsg( - painted_slices, resolution_, last_frame_id_, last_timestamp_); - - occupancy_grid_publisher_->publish(*msg_ptr); - } - - private: - const double resolution_; - - ::cartographer::common::Mutex mutex_; - ::rclcpp::Client::SharedPtr client_ GUARDED_BY(mutex_); - ::rclcpp::Subscription::SharedPtr submap_list_subscriber_ GUARDED_BY(mutex_); - ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ GUARDED_BY(mutex_); - - std::map submap_slices_ GUARDED_BY(mutex_); - ::rclcpp::TimerBase::SharedPtr occupancy_grid_publisher_timer_; - - std::string last_frame_id_; - ::rclcpp::Time last_timestamp_; -}; - -} // namespace -} // namespace cartographer_ros - -int main(int argc, char** argv) { - // Init rclcpp first because gflags reorders command line flags in argv - ::rclcpp::init(argc, argv); - - // Keep going if an unknown flag is encountered - // https://github.com/gflags/gflags/issues/148#issuecomment-318826625 - google::AllowCommandLineReparsing(); - google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, false); - - auto node = std::make_shared(FLAGS_resolution, FLAGS_publish_period_sec); - - rclcpp::spin(node); - ::rclcpp::shutdown(); - return 0; -} diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc deleted file mode 100644 index e5f40e769..000000000 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ /dev/null @@ -1,358 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * 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. - */ - -#include "cartographer_ros/offline_node.h" - -#include -#include -#include -#include -#include - -#include "cartographer_ros/node.h" -#include "cartographer_ros/playable_bag.h" -#include "cartographer_ros/split_string.h" -#include "cartographer_ros/urdf_reader.h" -#include "gflags/gflags.h" -#include "ros/callback_queue.h" -#include "rosgraph_msgs/Clock.h" -#include "tf2_ros/static_transform_broadcaster.h" -#include "urdf/model.h" - -DEFINE_string(configuration_directory, "", - "First directory in which configuration files are searched, " - "second is always the Cartographer installation to allow " - "including files from there."); -DEFINE_string( - configuration_basenames, "", - "Comma-separated list of basenames, i.e. not containing any " - "directory prefix, of the configuration files for each trajectory. " - "The first configuration file will be used for node options. " - "If less configuration files are specified than trajectories, the " - "first file will be used the remaining trajectories."); -DEFINE_string( - bag_filenames, "", - "Comma-separated list of bags to process. One bag per trajectory. " - "Any combination of simultaneous and sequential bags is supported."); -DEFINE_string(urdf_filenames, "", - "Comma-separated list of one or more URDF files that contain " - "static links for the sensor configuration(s)."); -DEFINE_bool(use_bag_transforms, true, - "Whether to read, use and republish transforms from bags."); -DEFINE_string(load_state_filename, "", - "If non-empty, filename of a .pbstream file to load, containing " - "a saved SLAM state."); -DEFINE_bool(load_frozen_state, true, - "Load the saved state as frozen (non-optimized) trajectories."); -DEFINE_bool(keep_running, false, - "Keep running the offline node after all messages from the bag " - "have been processed."); -DEFINE_double(skip_seconds, 0, - "Optional amount of seconds to skip from the beginning " - "(i.e. when the earliest bag starts.). "); - -namespace cartographer_ros { - -constexpr char kClockTopic[] = "clock"; -constexpr char kTfStaticTopic[] = "/tf_static"; -constexpr char kTfTopic[] = "tf"; -constexpr double kClockPublishFrequencySec = 1. / 30.; -constexpr int kSingleThreaded = 1; -// We publish tf messages one second earlier than other messages. Under -// the assumption of higher frequency tf this should ensure that tf can -// always interpolate. -const ::ros::Duration kDelay = ::ros::Duration(1.0); - -void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { - CHECK(!FLAGS_configuration_directory.empty()) - << "-configuration_directory is missing."; - CHECK(!FLAGS_configuration_basenames.empty()) - << "-configuration_basenames is missing."; - CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) - << "-bag_filenames and -load_state_filename cannot both be unspecified."; - const auto bag_filenames = - cartographer_ros::SplitString(FLAGS_bag_filenames, ','); - cartographer_ros::NodeOptions node_options; - const auto configuration_basenames = - cartographer_ros::SplitString(FLAGS_configuration_basenames, ','); - std::vector bag_trajectory_options(1); - std::tie(node_options, bag_trajectory_options.at(0)) = - LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0)); - - for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) { - TrajectoryOptions current_trajectory_options; - if (bag_index < configuration_basenames.size()) { - std::tie(std::ignore, current_trajectory_options) = LoadOptions( - FLAGS_configuration_directory, configuration_basenames.at(bag_index)); - } else { - current_trajectory_options = bag_trajectory_options.at(0); - } - bag_trajectory_options.push_back(current_trajectory_options); - } - if (bag_filenames.size() > 0) { - CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size()); - } - - // Since we preload the transform buffer, we should never have to wait for a - // transform. When we finish processing the bag, we will simply drop any - // remaining sensor data that cannot be transformed due to missing transforms. - node_options.lookup_transform_timeout_sec = 0.; - - auto map_builder = map_builder_factory(node_options.map_builder_options); - - const std::chrono::time_point start_time = - std::chrono::steady_clock::now(); - - tf2_ros::Buffer tf_buffer; - - std::vector urdf_transforms; - for (const std::string& urdf_filename : - cartographer_ros::SplitString(FLAGS_urdf_filenames, ',')) { - const auto current_urdf_transforms = - ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); - urdf_transforms.insert(urdf_transforms.end(), - current_urdf_transforms.begin(), - current_urdf_transforms.end()); - } - - tf_buffer.setUsingDedicatedThread(true); - - Node node(node_options, std::move(map_builder), &tf_buffer); - if (!FLAGS_load_state_filename.empty()) { - node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); - } - - ::ros::Publisher tf_publisher = - node.node_handle()->advertise( - kTfTopic, kLatestOnlyPublisherQueueSize); - - ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; - - ::ros::Publisher clock_publisher = - node.node_handle()->advertise( - kClockTopic, kLatestOnlyPublisherQueueSize); - - if (urdf_transforms.size() > 0) { - static_tf_broadcaster.sendTransform(urdf_transforms); - } - - ros::AsyncSpinner async_spinner(kSingleThreaded); - async_spinner.start(); - rosgraph_msgs::Clock clock; - auto clock_republish_timer = node.node_handle()->createWallTimer( - ::ros::WallDuration(kClockPublishFrequencySec), - [&clock_publisher, &clock](const ::ros::WallTimerEvent&) { - clock_publisher.publish(clock); - }, - false /* oneshot */, false /* autostart */); - - std::vector< - std::set> - bag_expected_sensor_ids; - if (configuration_basenames.size() == 1) { - const auto current_bag_expected_sensor_ids = - node.ComputeDefaultSensorIdsForMultipleBags( - {bag_trajectory_options.front()}); - bag_expected_sensor_ids = {bag_filenames.size(), - current_bag_expected_sensor_ids.front()}; - } else { - bag_expected_sensor_ids = - node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options); - } - CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size()); - - std::map, - cartographer::mapping::TrajectoryBuilderInterface::SensorId> - bag_topic_to_sensor_id; - PlayableBagMultiplexer playable_bag_multiplexer; - for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size(); - ++current_bag_index) { - const std::string& bag_filename = bag_filenames.at(current_bag_index); - if (!::ros::ok()) { - return; - } - for (const auto& expected_sensor_id : - bag_expected_sensor_ids.at(current_bag_index)) { - const auto bag_resolved_topic = std::make_pair( - static_cast(current_bag_index), - node.node_handle()->resolveName(expected_sensor_id.id)); - if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) { - LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag " - << current_bag_index << " resolves to topic " - << bag_resolved_topic.second << " which is already used by " - << " sensor " - << bag_topic_to_sensor_id.at(bag_resolved_topic).id; - } - bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id; - } - - playable_bag_multiplexer.AddPlayableBag(PlayableBag( - bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay, - // PlayableBag::FilteringEarlyMessageHandler is used to get an early - // peek at the tf messages in the bag and insert them into 'tf_buffer'. - // When a message is retrieved by GetNextMessage() further below, - // we will have already inserted further 'kDelay' seconds worth of - // transforms into 'tf_buffer' via this lambda. - [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) { - if (msg.isType()) { - if (FLAGS_use_bag_transforms) { - const auto tf_message = msg.instantiate(); - tf_publisher.publish(tf_message); - - for (const auto& transform : tf_message->transforms) { - try { - // We need to keep 'tf_buffer' small because it becomes very - // inefficient otherwise. We make sure that tf_messages are - // published before any data messages, so that tf lookups - // always work. - tf_buffer.setTransform(transform, "unused_authority", - msg.getTopic() == kTfStaticTopic); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } - } - } - // Tell 'PlayableBag' to filter the tf message since there is no - // further use for it. - return false; - } else { - return true; - } - })); - } - - // TODO(gaschler): Warn if resolved topics are not in bags. - std::unordered_map bag_index_to_trajectory_id; - const ros::Time begin_time = - // If no bags were loaded, we cannot peek the time of first message. - playable_bag_multiplexer.IsMessageAvailable() - ? playable_bag_multiplexer.PeekMessageTime() - : ros::Time(); - while (playable_bag_multiplexer.IsMessageAvailable()) { - if (!::ros::ok()) { - return; - } - - const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); - const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); - const int bag_index = std::get<1>(next_msg_tuple); - const bool is_last_message_in_bag = std::get<2>(next_msg_tuple); - - if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) { - continue; - } - - int trajectory_id; - // Lazily add trajectories only when the first message arrives in order - // to avoid blocking the sensor queue. - if (bag_index_to_trajectory_id.count(bag_index) == 0) { - trajectory_id = - node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index), - bag_trajectory_options.at(bag_index)); - CHECK(bag_index_to_trajectory_id - .emplace(std::piecewise_construct, - std::forward_as_tuple(bag_index), - std::forward_as_tuple(trajectory_id)) - .second); - LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag " - << bag_filenames.at(bag_index); - } else { - trajectory_id = bag_index_to_trajectory_id.at(bag_index); - } - - const auto bag_topic = std::make_pair( - bag_index, - node.node_handle()->resolveName(msg.getTopic(), false /* resolve */)); - auto it = bag_topic_to_sensor_id.find(bag_topic); - if (it != bag_topic_to_sensor_id.end()) { - const std::string& sensor_id = it->second.id; - if (msg.isType()) { - node.HandleLaserScanMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleMultiEchoLaserScanMessage( - trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandlePointCloud2Message( - trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleImuMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleOdometryMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleNavSatFixMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleLandmarkMessage( - trajectory_id, sensor_id, - msg.instantiate()); - } - } - clock.clock = msg.getTime(); - clock_publisher.publish(clock); - - if (is_last_message_in_bag) { - node.FinishTrajectory(trajectory_id); - } - } - - // Ensure the clock is republished after the bag has been finished, during the - // final optimization, serialization, and optional indefinite spinning at the - // end. - clock_republish_timer.start(); - node.RunFinalOptimization(); - - const std::chrono::time_point end_time = - std::chrono::steady_clock::now(); - const double wall_clock_seconds = - std::chrono::duration_cast>(end_time - - start_time) - .count(); - - LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s"; -#ifdef __linux__ - timespec cpu_timespec = {}; - clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec); - LOG(INFO) << "Elapsed CPU time: " - << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s"; - rusage usage; - CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno); - LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB"; -#endif - - if (::ros::ok() && bag_filenames.size() > 0) { - const std::string output_filename = bag_filenames.front(); - const std::string suffix = ".pbstream"; - const std::string state_output_filename = output_filename + suffix; - LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; - node.SerializeState(state_output_filename); - } - if (FLAGS_keep_running) { - ::ros::waitForShutdown(); - } -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc deleted file mode 100644 index d9d17edbb..000000000 --- a/cartographer_ros/cartographer_ros/playable_bag.cc +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * 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. - */ - -#include "cartographer_ros/playable_bag.h" - -#include "cartographer/common/make_unique.h" -#include "cartographer_ros/node_constants.h" -#include "glog/logging.h" -#include "tf2_msgs/TFMessage.h" - -namespace cartographer_ros { - -PlayableBag::PlayableBag( - const std::string& bag_filename, const int bag_id, - const ros::Time start_time, const ros::Time end_time, - const ros::Duration buffer_delay, - FilteringEarlyMessageHandler filtering_early_message_handler) - : bag_(cartographer::common::make_unique( - bag_filename, rosbag::bagmode::Read)), - view_(cartographer::common::make_unique(*bag_, start_time, - end_time)), - view_iterator_(view_->begin()), - finished_(false), - bag_id_(bag_id), - bag_filename_(bag_filename), - duration_in_seconds_( - (view_->getEndTime() - view_->getBeginTime()).toSec()), - log_counter_(0), - buffer_delay_(buffer_delay), - filtering_early_message_handler_( - std::move(filtering_early_message_handler)) { - AdvanceUntilMessageAvailable(); -} - -ros::Time PlayableBag::PeekMessageTime() const { - CHECK(IsMessageAvailable()); - return buffered_messages_.front().getTime(); -} - -std::tuple PlayableBag::GetBeginEndTime() const { - return std::make_tuple(view_->getBeginTime(), view_->getEndTime()); -} - -rosbag::MessageInstance PlayableBag::GetNextMessage() { - CHECK(IsMessageAvailable()); - const rosbag::MessageInstance msg = buffered_messages_.front(); - buffered_messages_.pop_front(); - AdvanceUntilMessageAvailable(); - if ((log_counter_++ % 10000) == 0) { - LOG(INFO) << "Processed " << (msg.getTime() - view_->getBeginTime()).toSec() - << " of " << duration_in_seconds_ << " seconds of bag " - << bag_filename_; - } - return msg; -} - -bool PlayableBag::IsMessageAvailable() const { - return !buffered_messages_.empty() && - (buffered_messages_.front().getTime() < - buffered_messages_.back().getTime() - buffer_delay_); -} - -int PlayableBag::bag_id() const { return bag_id_; } - -void PlayableBag::AdvanceOneMessage() { - CHECK(!finished_); - if (view_iterator_ == view_->end()) { - finished_ = true; - return; - } - rosbag::MessageInstance& msg = *view_iterator_; - if (!filtering_early_message_handler_ || - filtering_early_message_handler_(msg)) { - buffered_messages_.push_back(msg); - } - ++view_iterator_; -} - -void PlayableBag::AdvanceUntilMessageAvailable() { - if (finished_) { - return; - } - do { - AdvanceOneMessage(); - } while (!finished_ && !IsMessageAvailable()); -} - -void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { - playable_bags_.push_back(std::move(playable_bag)); - CHECK(playable_bags_.back().IsMessageAvailable()); - next_message_queue_.emplace( - BagMessageItem{playable_bags_.back().PeekMessageTime(), - static_cast(playable_bags_.size() - 1)}); -} - -bool PlayableBagMultiplexer::IsMessageAvailable() const { - return !next_message_queue_.empty(); -} - -std::tuple -PlayableBagMultiplexer::GetNextMessage() { - CHECK(IsMessageAvailable()); - const int current_bag_index = next_message_queue_.top().bag_index; - PlayableBag& current_bag = playable_bags_.at(current_bag_index); - rosbag::MessageInstance msg = current_bag.GetNextMessage(); - CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp); - next_message_queue_.pop(); - if (current_bag.IsMessageAvailable()) { - next_message_queue_.emplace( - BagMessageItem{current_bag.PeekMessageTime(), current_bag_index}); - } - return std::make_tuple(std::move(msg), current_bag.bag_id(), - !current_bag.IsMessageAvailable()); -} - -ros::Time PlayableBagMultiplexer::PeekMessageTime() const { - CHECK(IsMessageAvailable()); - return next_message_queue_.top().message_timestamp; -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/split_string.cc b/cartographer_ros/cartographer_ros/split_string.cc deleted file mode 100644 index 4924e6920..000000000 --- a/cartographer_ros/cartographer_ros/split_string.cc +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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. - */ - -#include "cartographer_ros/split_string.h" - -#include - -namespace cartographer_ros { - -std::vector SplitString(const std::string& input, - const char delimiter) { - std::istringstream stream(input); - std::string token; - std::vector tokens; - while (std::getline(stream, token, delimiter)) { - tokens.push_back(token); - } - return tokens; -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/split_string.h b/cartographer_ros/cartographer_ros/split_string.h deleted file mode 100644 index f12afbb8f..000000000 --- a/cartographer_ros/cartographer_ros/split_string.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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 CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SPLIT_STRING_H -#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SPLIT_STRING_H - -#include -#include - -namespace cartographer_ros { - -// Split 'input' at 'delimiter'. -std::vector SplitString(const std::string& input, char delimiter); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SPLIT_STRING_H diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc deleted file mode 100644 index 2930c686c..000000000 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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. - */ - -#include -#include - -#include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/make_unique.h" -#include "cartographer/common/port.h" -#include "cartographer_ros/node_constants.h" -#include "cartographer_ros/ros_log_sink.h" -#include "cartographer_ros/trajectory_options.h" -#include "cartographer_ros_msgs/StartTrajectory.h" -#include "cartographer_ros_msgs/StatusCode.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" -#include "gflags/gflags.h" -#include "ros/ros.h" - -DEFINE_string(configuration_directory, "", - "First directory in which configuration files are searched, " - "second is always the Cartographer installation to allow " - "including files from there."); -DEFINE_string(configuration_basename, "", - "Basename, i.e. not containing any directory prefix, of the " - "configuration file."); - -DEFINE_string(initial_pose, "", "Starting pose of a new trajectory"); - -namespace cartographer_ros { -namespace { - -TrajectoryOptions LoadOptions() { - auto file_resolver = cartographer::common::make_unique< - cartographer::common::ConfigurationFileResolver>( - std::vector{FLAGS_configuration_directory}); - const std::string code = - file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); - auto lua_parameter_dictionary = - cartographer::common::LuaParameterDictionary::NonReferenceCounted( - code, std::move(file_resolver)); - if (!FLAGS_initial_pose.empty()) { - auto initial_trajectory_pose_file_resolver = - cartographer::common::make_unique< - cartographer::common::ConfigurationFileResolver>( - std::vector{FLAGS_configuration_directory}); - auto initial_trajectory_pose = - cartographer::common::LuaParameterDictionary::NonReferenceCounted( - "return " + FLAGS_initial_pose, - std::move(initial_trajectory_pose_file_resolver)); - return CreateTrajectoryOptions(lua_parameter_dictionary.get(), - initial_trajectory_pose.get()); - } else { - return CreateTrajectoryOptions(lua_parameter_dictionary.get()); - } -} - -bool Run() { - ros::NodeHandle node_handle; - ros::ServiceClient client = - node_handle.serviceClient( - kStartTrajectoryServiceName); - cartographer_ros_msgs::StartTrajectory srv; - srv.request.options = ToRosMessage(LoadOptions()); - srv.request.topics.laser_scan_topic = node_handle.resolveName( - kLaserScanTopic, true /* apply topic remapping */); - srv.request.topics.multi_echo_laser_scan_topic = - node_handle.resolveName(kMultiEchoLaserScanTopic, true); - srv.request.topics.point_cloud2_topic = - node_handle.resolveName(kPointCloud2Topic, true); - srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true); - srv.request.topics.odometry_topic = - node_handle.resolveName(kOdometryTopic, true); - - if (!client.call(srv)) { - LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << "."; - return false; - } - if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) { - LOG(ERROR) << "Error starting trajectory - message: '" - << srv.response.status.message - << "' (status code: " << std::to_string(srv.response.status.code) - << ")."; - return false; - } - LOG(INFO) << "Started trajectory " << srv.response.trajectory_id; - return true; -} - -} // namespace -} // namespace cartographer_ros - -int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - google::SetUsageMessage( - "\n\n" - "Convenience tool around the start_trajectory service. This takes a Lua " - "file that is accepted by the node as well and starts a new trajectory " - "using its settings.\n"); - google::ParseCommandLineFlags(&argc, &argv, true); - - CHECK(!FLAGS_configuration_directory.empty()) - << "-configuration_directory is missing."; - CHECK(!FLAGS_configuration_basename.empty()) - << "-configuration_basename is missing."; - - ::ros::init(argc, argv, "cartographer_start_trajectory"); - ::ros::start(); - - cartographer_ros::ScopedRosLogSink ros_log_sink; - int exit_code = cartographer_ros::Run() ? 0 : 1; - ::ros::shutdown(); - return exit_code; -} diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc deleted file mode 100644 index 68fb352a8..000000000 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * 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. - */ - -#include "cartographer_ros/trajectory_options.h" - -#include "cartographer/mapping/trajectory_builder_interface.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer/transform/transform.h" -#include "cartographer_ros/time_conversion.h" -#include "glog/logging.h" - -#include "rclcpp/clock.hpp" - -namespace cartographer_ros { - -namespace { - -void CheckTrajectoryOptions(const TrajectoryOptions& options) { - CHECK_GE(options.num_subdivisions_per_laser_scan, 1); - CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans + - options.num_point_clouds, - 1) - << "Configuration error: 'num_laser_scans', " - "'num_multi_echo_laser_scans' and 'num_point_clouds' are " - "all zero, but at least one is required."; -} - -} // namespace - -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* const - lua_parameter_dictionary) { - TrajectoryOptions options; - options.trajectory_builder_options = - ::cartographer::mapping::CreateTrajectoryBuilderOptions( - lua_parameter_dictionary->GetDictionary("trajectory_builder").get()); - options.tracking_frame = - lua_parameter_dictionary->GetString("tracking_frame"); - options.published_frame = - lua_parameter_dictionary->GetString("published_frame"); - options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); - options.provide_odom_frame = - lua_parameter_dictionary->GetBool("provide_odom_frame"); - options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); - options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); - options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks"); - options.publish_frame_projected_to_2d = - lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d"); - options.num_laser_scans = - lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); - options.num_multi_echo_laser_scans = - lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans"); - options.num_subdivisions_per_laser_scan = - lua_parameter_dictionary->GetNonNegativeInt( - "num_subdivisions_per_laser_scan"); - options.num_point_clouds = - lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds"); - options.rangefinder_sampling_ratio = - lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio"); - options.odometry_sampling_ratio = - lua_parameter_dictionary->GetDouble("odometry_sampling_ratio"); - options.fixed_frame_pose_sampling_ratio = - lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio"); - options.imu_sampling_ratio = - lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); - options.landmarks_sampling_ratio = - lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio"); - CheckTrajectoryOptions(options); - return options; -} - -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose, - ::rclcpp::Time node_time) { - TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary); - *options.trajectory_builder_options.mutable_initial_trajectory_pose() = - CreateInitialTrajectoryPose(initial_trajectory_pose, node_time); - return options; -} - -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::rclcpp::Time node_time) { - ::cartographer::mapping::proto::InitialTrajectoryPose pose; - pose.set_to_trajectory_id( - lua_parameter_dictionary->GetNonNegativeInt("to_trajectory_id")); - *pose.mutable_relative_pose() = - cartographer::transform::ToProto(cartographer::transform::FromDictionary( - lua_parameter_dictionary->GetDictionary("relative_pose").get())); - pose.set_timestamp( - lua_parameter_dictionary->HasKey("timestamp") - ? lua_parameter_dictionary->GetNonNegativeInt("timestamp") - : cartographer::common::ToUniversal(FromRos(node_time))); - return pose; -} - -bool FromRosMessage(const cartographer_ros_msgs::msg::TrajectoryOptions& msg, - TrajectoryOptions* options) { - options->tracking_frame = msg.tracking_frame; - options->published_frame = msg.published_frame; - options->odom_frame = msg.odom_frame; - options->provide_odom_frame = msg.provide_odom_frame; - options->use_odometry = msg.use_odometry; - options->use_nav_sat = msg.use_nav_sat; - options->use_landmarks = msg.use_landmarks; - options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d; - options->num_laser_scans = msg.num_laser_scans; - options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans; - options->num_subdivisions_per_laser_scan = - msg.num_subdivisions_per_laser_scan; - options->num_point_clouds = msg.num_point_clouds; - options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio; - options->odometry_sampling_ratio = msg.odometry_sampling_ratio; - options->fixed_frame_pose_sampling_ratio = - msg.fixed_frame_pose_sampling_ratio; - options->imu_sampling_ratio = msg.imu_sampling_ratio; - options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio; - if (!options->trajectory_builder_options.ParseFromString( - msg.trajectory_builder_options_proto)) { - LOG(ERROR) << "Failed to parse protobuf"; - return false; - } - CheckTrajectoryOptions(*options); - return true; -} - -cartographer_ros_msgs::msg::TrajectoryOptions ToRosMessage( - const TrajectoryOptions& options) { - cartographer_ros_msgs::msg::TrajectoryOptions msg; - msg.tracking_frame = options.tracking_frame; - msg.published_frame = options.published_frame; - msg.odom_frame = options.odom_frame; - msg.provide_odom_frame = options.provide_odom_frame; - msg.use_odometry = options.use_odometry; - msg.use_nav_sat = options.use_nav_sat; - msg.use_landmarks = options.use_landmarks; - msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d; - msg.num_laser_scans = options.num_laser_scans; - msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans; - msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan; - msg.num_point_clouds = options.num_point_clouds; - msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio; - msg.odometry_sampling_ratio = options.odometry_sampling_ratio; - msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio; - msg.imu_sampling_ratio = options.imu_sampling_ratio; - msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio; - options.trajectory_builder_options.SerializeToString( - &msg.trajectory_builder_options_proto); - return msg; -} - -} // namespace cartographer_ros diff --git a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua index de48eb086..9a2d3bf8e 100644 --- a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua +++ b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua @@ -87,7 +87,7 @@ options = { -- We also write a PLY file at this stage, because gray points look good. -- The points in the PLY can be visualized using - -- https://github.com/googlecartographer/point_cloud_viewer. + -- https://github.com/cartographer-project/point_cloud_viewer. { action = "write_ply", filename = "points.ply", diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 85699dbfe..8d29069b6 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/backpack_2d_localization.lua b/cartographer_ros/configuration_files/backpack_2d_localization.lua index 9ecb218ab..c5da92ca1 100644 --- a/cartographer_ros/configuration_files/backpack_2d_localization.lua +++ b/cartographer_ros/configuration_files/backpack_2d_localization.lua @@ -14,7 +14,9 @@ include "backpack_2d.lua" -TRAJECTORY_BUILDER.pure_localization = true +TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, +} POSE_GRAPH.optimize_every_n_nodes = 20 return options diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index ad730bab3..b5c05f329 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/backpack_3d_localization.lua b/cartographer_ros/configuration_files/backpack_3d_localization.lua index e1f4e9fb2..3e677f569 100644 --- a/cartographer_ros/configuration_files/backpack_3d_localization.lua +++ b/cartographer_ros/configuration_files/backpack_3d_localization.lua @@ -14,7 +14,9 @@ include "backpack_3d.lua" -TRAJECTORY_BUILDER.pure_localization = true +TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, +} POSE_GRAPH.optimize_every_n_nodes = 100 return options diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 85125cd66..92e4708b9 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -1,104 +1,55 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /Submaps1 - - /PointCloud21 - Splitter Ratio: 0.600671 - Tree Height: 821 - - Class: rviz/Selection + - /Global Options1 + - /RobotModel1/Description Topic1 + - /TF1/Tree1 + - /map1/Topic1 + - /SubmapsDisplay1/Submaps1 + - /Trajectories1/Namespaces1 + - /Constraints1/Namespaces1 + Splitter Ratio: 0.42203986644744873 + Tree Height: 1174 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: teleop_panel/Teleop + Name: Teleop + Topic: /rt_cmd_vel + - Class: wyca_rviz_plugin/Wyca + Name: Wyca + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - horizontal_laser_link: - Value: true - imu_link: - Value: true - map: - Value: true - odom: - Value: true - vertical_laser_link: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - odom: - base_link: - horizontal_laser_link: - {} - imu_link: - {} - vertical_laser_link: - {} - Update Interval: 0 - Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -106,38 +57,45 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - horizontal_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - vertical_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: Submaps + - Class: rviz_default_plugins/TF Enabled: true - Name: Submaps - Submap query service: /submap_query - Topic: /submap_list - Tracking frame: base_link - Unreliable: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 0.30000001192092896 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -147,8 +105,8 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 0 Enabled: true @@ -159,93 +117,343 @@ Visualization Manager: Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /scan_matched_points2 - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_matched_points2 Use Fixed Frame: true - Use rainbow: true + Use rainbow: false Value: true - - Class: rviz/MarkerArray + - Class: cartographer_rviz/SubmapsDisplay Enabled: true - Marker Topic: /trajectory_node_list + Fade-out distance: 1 + High Resolution: true + Low Resolution: false + Name: SubmapsDisplay + Submap query service: /submap_query + Submaps: + All: true + All Submap Pose Markers: true + Trajectory 0: + 0.60: true + 1.60: true + 10.60: true + 100.60: true + 101.60: true + 102.60: true + 103.60: true + 104.60: true + 105.60: true + 106.60: true + 107.60: true + 108.60: true + 109.60: true + 11.60: true + 110.60: true + 111.60: true + 112.60: true + 113.60: true + 114.60: true + 115.60: true + 116.60: true + 117.60: true + 118.60: true + 119.60: true + 12.60: true + 120.60: true + 121.60: true + 122.60: true + 123.60: true + 124.60: true + 125.60: true + 126.60: true + 127.60: true + 128.60: true + 129.60: true + 13.60: true + 130.60: true + 131.60: true + 132.60: true + 133.60: true + 134.60: true + 135.60: true + 136.60: true + 137.60: true + 138.60: true + 139.60: true + 14.60: true + 140.60: true + 141.60: true + 142.60: true + 143.60: true + 144.60: true + 145.60: true + 146.60: true + 147.60: true + 148.60: true + 149.60: true + 15.60: true + 150.60: true + 151.60: true + 152.60: true + 153.60: true + 154.60: true + 155.60: true + 156.60: true + 157.60: true + 158.60: true + 159.60: true + 16.60: true + 160.60: true + 161.60: true + 162.60: true + 163.60: true + 164.60: true + 165.60: true + 166.60: true + 167.60: true + 168.60: true + 169.60: true + 17.60: true + 170.60: true + 171.60: true + 172.43: true + 173.13: true + 18.60: true + 19.60: true + 2.60: true + 20.60: true + 21.60: true + 22.60: true + 23.60: true + 24.60: true + 25.60: true + 26.60: true + 27.60: true + 28.60: true + 29.60: true + 3.60: true + 30.60: true + 31.60: true + 32.60: true + 33.60: true + 34.60: true + 35.60: true + 36.60: true + 37.60: true + 38.60: true + 39.60: true + 4.60: true + 40.60: true + 41.60: true + 42.60: true + 43.60: true + 44.60: true + 45.60: true + 46.60: true + 47.60: true + 48.60: true + 49.60: true + 5.60: true + 50.60: true + 51.60: true + 52.60: true + 53.60: true + 54.60: true + 55.60: true + 56.60: true + 57.60: true + 58.60: true + 59.60: true + 6.60: true + 60.60: true + 61.60: true + 62.60: true + 63.60: true + 64.60: true + 65.60: true + 66.60: true + 67.60: true + 68.60: true + 69.60: true + 7.60: true + 70.60: true + 71.60: true + 72.60: true + 73.60: true + 74.60: true + 75.60: true + 76.60: true + 77.60: true + 78.60: true + 79.60: true + 8.60: true + 80.60: true + 81.60: true + 82.60: true + 83.60: true + 84.60: true + 85.60: true + 86.60: true + 87.60: true + 88.60: true + 89.60: true + 9.60: true + 90.60: true + 91.60: true + 92.60: true + 93.60: true + 94.60: true + 95.60: true + 96.60: true + 97.60: true + 98.60: true + 99.60: true + Submap Pose Markers: true + Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /submap_list + Tracking frame: map + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false Name: Trajectories Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_node_list + Value: false + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /landmark_poses_list - Name: Landmark Poses + Name: Constraints Namespaces: - "": true - Queue Size: 100 + Inter constraints, different trajectories: true + Inter constraints, same trajectory: true + Inter residuals, different trajectories: true + Inter residuals, same trajectory: true + Intra constraints: true + Intra residuals: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /constraint_list Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /constraint_list - Name: Constraints + Name: Landmarks Namespaces: - "": true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /landmark_poses_list Value: true Enabled: true Global Options: - Background Color: 100; 100; 100 + Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: 0 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 204.58663940429688 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: -11.516843795776367 + Y: 13.40983772277832 + Z: 7.403941708616912e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 10 - Target Frame: - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: map + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 1.6953976154327393 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1123 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000028100000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000006640000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1918 - X: 0 - Y: 24 + Width: 2560 + Wyca: + collapsed: false + X: 3840 + Y: 0 diff --git a/cartographer_ros/configuration_files/demo_3d.rviz b/cartographer_ros/configuration_files/demo_3d.rviz index e061b6a91..af4523b0e 100644 --- a/cartographer_ros/configuration_files/demo_3d.rviz +++ b/cartographer_ros/configuration_files/demo_3d.rviz @@ -1,76 +1,92 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Submaps1 - - /PointCloud23 - - /PointCloud23/Autocompute Value Bounds1 - Splitter Ratio: 0.600671 - Tree Height: 817 - - Class: rviz/Selection + - /RobotModel1/Description Topic1 + - /map1/Status1 + - /map1/Topic1 + - /SubmapsDisplay1/Submaps1 + - /Trajectories1/Namespaces1 + Splitter Ratio: 0.47058823704719543 + Tree Height: 1174 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: teleop_panel/Teleop + Name: Teleop + Topic: /rt_cmd_vel + - Class: wyca_rviz_plugin/Wyca + Name: Wyca + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: matched_points Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + horizontal_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + vertical_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 Value: true - - Class: rviz/TF + Visual Enabled: true + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - All Enabled: true + All Enabled: false base_link: - Value: true + Value: false horizontal_vlp16_link: Value: true imu_link: @@ -81,7 +97,7 @@ Visualization Manager: Value: true vertical_vlp16_link: Value: true - Marker Scale: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true @@ -98,216 +114,281 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - horizontal_vlp16_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - vertical_vlp16_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 + Name: map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true - Visual Enabled: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 7.6265 - Min Value: 5.66667 + Max Value: 10 + Min Value: -10 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 20; 220; 20 + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 Color Transformer: FlatColor - Decay Time: 0.1 - Enabled: false + Decay Time: 0 + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: PointCloud2 + Name: matched_points Position Transformer: XYZ - Queue Size: 200 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /horizontal_laser_3d - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_matched_points2 Use Fixed Frame: true - Use rainbow: true + Use rainbow: false + Value: true + - Class: cartographer_rviz/SubmapsDisplay + Enabled: true + Fade-out distance: 1 + High Resolution: true + Low Resolution: false + Name: SubmapsDisplay + Submap query service: /submap_query + Submaps: + All: true + All Submap Pose Markers: true + Trajectory 0: + 0.320: true + 1.204: true + 2.44: true + Submap Pose Markers: true + Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /submap_list + Tracking frame: odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_node_list Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Constraints + Namespaces: + Inter constraints, different trajectories: true + Inter constraints, same trajectory: true + Inter residuals, different trajectories: true + Inter residuals, same trajectory: true + Intra constraints: true + Intra residuals: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /constraint_list + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Landmarks + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /landmark_poses_list + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 5.6087 - Min Value: 3.77875 + Max Value: 10 + Min Value: -10 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 240; 220; 20 - Color Transformer: FlatColor - Decay Time: 0.1 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 Enabled: false - Invert Rainbow: true + Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 57 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 + Min Intensity: 1 + Name: horizontal_laser Position Transformer: XYZ - Queue Size: 200 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /vertical_laser_3d - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /horizontal_laser_3d Use Fixed Frame: true Use rainbow: true Value: false - - Class: Submaps - Enabled: true - Name: Submaps - Submap query service: /submap_query - Topic: /submap_list - Tracking frame: base_link - Unreliable: false - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 - Value: false + Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 - Color Transformer: AxisColor + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 90 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 + Min Intensity: 1 + Name: vertical_laser Position Transformer: XYZ - Queue Size: 20 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /scan_matched_points2 - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vertical_laser_3d Use Fixed Frame: true Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /trajectory_node_list - Name: Trajectories - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /landmark_poses_list - Name: Landmark Poses - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /constraint_list - Name: Constraints - Namespaces: - "": true - Queue Size: 100 - Value: true + Value: false Enabled: true Global Options: - Background Color: 100; 100; 100 + Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: 0 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 59.301063537597656 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: -0.8213143348693848 + Y: -0.22988438606262207 + Z: -0.00012816624075639993 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 10 - Target Frame: - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1647964715957642 + Target Frame: map + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 0.3154185116291046 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1123 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000007390000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1918 - X: 0 - Y: 24 + Width: 2560 + Wyca: + collapsed: false + X: 3840 + Y: 0 diff --git a/cartographer_ros/configuration_files/mir-100-mapping.lua b/cartographer_ros/configuration_files/mir-100-mapping.lua new file mode 100644 index 000000000..1ed5087fe --- /dev/null +++ b/cartographer_ros/configuration_files/mir-100-mapping.lua @@ -0,0 +1,77 @@ +-- Copyright 2018 The Cartographer Authors +-- +-- 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. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_frame", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_pose_extrapolator = true, + use_nav_sat = false, + use_landmarks = true, + num_laser_scans = 2, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +TRAJECTORY_BUILDER.collate_landmarks = false +TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 2 +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 45 + +-- more points +TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.2 +TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 400 +-- slightly slower insertion +TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.53 +TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.493 +-- slightly shorter rays +TRAJECTORY_BUILDER_2D.max_range = 15. +-- wheel odometry is fine +TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 20 +-- IMU is ok +TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 20 + +-- less outliers +POSE_GRAPH.constraint_builder.max_constraint_distance = 5. +POSE_GRAPH.constraint_builder.min_score = 0.5 +-- tune down IMU in optimization +POSE_GRAPH.optimization_problem.acceleration_weight = 0.1 * 1e3 +POSE_GRAPH.optimization_problem.rotation_weight = 0.1 * 3e5 +-- ignore wheels in optimization +POSE_GRAPH.optimization_problem.odometry_translation_weight = 0. +POSE_GRAPH.optimization_problem.odometry_rotation_weight = 0. +POSE_GRAPH.optimization_problem.log_solver_summary = true + +return options + diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 31ec6141d..ed6d473f3 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index f6935e992..c9f5ab00c 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index d1cd4c327..109701873 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = false, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = true, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/include/cartographer_ros/assets_writer.h similarity index 98% rename from cartographer_ros/cartographer_ros/assets_writer.h rename to cartographer_ros/include/cartographer_ros/assets_writer.h index c12a87f07..4e3ec5149 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/include/cartographer_ros/assets_writer.h @@ -21,6 +21,7 @@ #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/include/cartographer_ros/map_builder_bridge.h similarity index 61% rename from cartographer_ros/cartographer_ros/map_builder_bridge.h rename to cartographer_ros/include/cartographer_ros/map_builder_bridge.h index 82e23f0cb..b2c00b751 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/include/cartographer_ros/map_builder_bridge.h @@ -22,8 +22,9 @@ #include #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer_ros/node_options.h" @@ -33,17 +34,23 @@ #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" -#include -#include +#include "cartographer_ros_msgs/srv/trajectory_query.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" -#include +// Abseil unfortunately pulls in winnt.h, which #defines DELETE. +// Clean up to unbreak visualization_msgs::msg::Marker::DELETE. +#ifdef DELETE +#undef DELETE +#endif +#include "visualization_msgs/msg/marker_array.hpp" namespace cartographer_ros { class MapBuilderBridge { public: - struct TrajectoryState { - // Contains the trajectory state data received from local SLAM, after + struct LocalTrajectoryData { + // Contains the trajectory data received from local SLAM, after // it had processed accumulated 'range_data_in_local' and estimated // current 'local_pose' at 'time'. struct LocalSlamData { @@ -73,35 +80,40 @@ class MapBuilderBridge { const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); void RunFinalOptimization(); - bool SerializeState(const std::string& filename); + bool SerializeState(const std::string& filename, + const bool include_unfinished_submaps); void HandleSubmapQuery( - const std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Response> response); - - std::set GetFrozenTrajectoryIds(); - cartographer_ros_msgs::msg::SubmapList GetSubmapList(::rclcpp::Time node_time); - std::unordered_map GetTrajectoryStates() - EXCLUDES(mutex_); - visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(::rclcpp::Time node_time); - visualization_msgs::msg::MarkerArray GetLandmarkPosesList(::rclcpp::Time node_time); - visualization_msgs::msg::MarkerArray GetConstraintList(::rclcpp::Time node_time); + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response); + void HandleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response); + + std::map + GetTrajectoryStates(); + cartographer_ros_msgs::msg::SubmapList GetSubmapList(rclcpp::Time node_time); + std::unordered_map GetLocalTrajectoryData() + LOCKS_EXCLUDED(mutex_); + visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetLandmarkPosesList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetConstraintList(rclcpp::Time node_time); SensorBridge* sensor_bridge(int trajectory_id); private: - void OnLocalSlamResult( - const int trajectory_id, const ::cartographer::common::Time time, - const ::cartographer::transform::Rigid3d local_pose, - ::cartographer::sensor::RangeData range_data_in_local, - const std::unique_ptr - insertion_result) EXCLUDES(mutex_); - - cartographer::common::Mutex mutex_; + void OnLocalSlamResult(const int trajectory_id, + const ::cartographer::common::Time time, + const ::cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local) + LOCKS_EXCLUDED(mutex_); + + absl::Mutex mutex_; const NodeOptions node_options_; - std::unordered_map> - trajectory_state_data_ GUARDED_BY(mutex_); + std::unordered_map> + local_slam_data_ GUARDED_BY(mutex_); std::unique_ptr map_builder_; tf2_ros::Buffer* const tf_buffer_; diff --git a/cartographer_ros/include/cartographer_ros/metrics/family_factory.h b/cartographer_ros/include/cartographer_ros/metrics/family_factory.h new file mode 100644 index 000000000..0c941e5a3 --- /dev/null +++ b/cartographer_ros/include/cartographer_ros/metrics/family_factory.h @@ -0,0 +1,61 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H +#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H + +#include +#include + +#include "cartographer/metrics/family_factory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/family.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "cartographer_ros_msgs/srv/read_metrics.hpp" + +namespace cartographer_ros { +namespace metrics { + +// Realizes the factory / registry interface for the metrics in libcartographer +// and provides a wrapper to collect ROS messages from the metrics it owns. +class FamilyFactory : public ::cartographer::metrics::FamilyFactory { + public: + ::cartographer::metrics::Family<::cartographer::metrics::Counter>* + + NewCounterFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Gauge>* + NewGaugeFamily(const std::string& name, + const std::string& description) override; + ::cartographer::metrics::Family<::cartographer::metrics::Histogram>* + NewHistogramFamily(const std::string& name, const std::string& description, + const ::cartographer::metrics::Histogram::BucketBoundaries& + boundaries) override; + + void ReadMetrics( + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const; + + private: + std::vector> counter_families_; + std::vector> gauge_families_; + std::vector> histogram_families_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H diff --git a/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h b/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h new file mode 100644 index 000000000..3ef5666d9 --- /dev/null +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h @@ -0,0 +1,51 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H + +#include "cartographer/metrics/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros_msgs/msg/metric.hpp" + +namespace cartographer_ros { +namespace metrics { + +class Counter : public ::cartographer::metrics::Counter { + public: + explicit Counter(const std::map& labels) + : gauge_(labels) {} + + void Increment(const double value) override { gauge_.Increment(value); } + + void Increment() override { gauge_.Increment(); } + + double Value() { return gauge_.Value(); } + + cartographer_ros_msgs::msg::Metric ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg = gauge_.ToRosMessage(); + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_COUNTER; + return msg; + } + + private: + Gauge gauge_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H diff --git a/cartographer_ros/include/cartographer_ros/metrics/internal/family.h b/cartographer_ros/include/cartographer_ros/metrics/internal/family.h new file mode 100644 index 000000000..bcab328a1 --- /dev/null +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/family.h @@ -0,0 +1,81 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H + +#include +#include + +#include "cartographer/metrics/family_factory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "cartographer_ros_msgs/msg/metric_family.hpp" + +namespace cartographer_ros { +namespace metrics { +class CounterFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Counter> { + public: + CounterFamily(const std::string& name, const std::string& description) + : name_(name), description_(description) {} + Counter* Add(const std::map& labels) override; + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; +}; + +class GaugeFamily + : public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> { + public: + GaugeFamily(const std::string& name, const std::string& description) + : name_(name), description_(description) {} + Gauge* Add(const std::map& labels) override; + + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; +}; + +class HistogramFamily : public ::cartographer::metrics::Family< + ::cartographer::metrics::Histogram> { + public: + HistogramFamily(const std::string& name, const std::string& description, + const BucketBoundaries& boundaries) + : name_(name), description_(description), boundaries_(boundaries) {} + + Histogram* Add(const std::map& labels) override; + + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); + + private: + std::string name_; + std::string description_; + std::vector> wrappers_; + const BucketBoundaries boundaries_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H diff --git a/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h b/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h new file mode 100644 index 000000000..f311ab133 --- /dev/null +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h @@ -0,0 +1,80 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H + +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/metrics/gauge.h" +#include "cartographer_ros_msgs/msg/metric.hpp" + +namespace cartographer_ros { +namespace metrics { + +class Gauge : public ::cartographer::metrics::Gauge { + public: + explicit Gauge(const std::map& labels) + : labels_(labels), value_(0.) {} + + void Decrement(const double value) override { Add(-1. * value); } + + void Decrement() override { Decrement(1.); } + + void Increment(const double value) override { Add(value); } + + void Increment() override { Increment(1.); } + + void Set(double value) override { + absl::MutexLock lock(&mutex_); + value_ = value; + } + + double Value() { + absl::MutexLock lock(&mutex_); + return value_; + } + + cartographer_ros_msgs::msg::Metric ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg; + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_GAUGE; + for (const auto& label : labels_) { + cartographer_ros_msgs::msg::MetricLabel label_msg; + label_msg.key = label.first; + label_msg.value = label.second; + msg.labels.push_back(label_msg); + } + msg.value = Value(); + return msg; + } + + private: + void Add(const double value) { + absl::MutexLock lock(&mutex_); + value_ += value; + } + + absl::Mutex mutex_; + const std::map labels_; + double value_ GUARDED_BY(mutex_); +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H diff --git a/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h b/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h new file mode 100644 index 000000000..b5d84045c --- /dev/null +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h @@ -0,0 +1,60 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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 CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H +#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H + +#include +#include + +#include "absl/synchronization/mutex.h" +#include "cartographer/metrics/histogram.h" +#include "cartographer_ros_msgs/msg/metric.hpp" + +namespace cartographer_ros { +namespace metrics { + +constexpr double kInfiniteBoundary = std::numeric_limits::infinity(); + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +class Histogram : public ::cartographer::metrics::Histogram { + public: + explicit Histogram(const std::map& labels, + const BucketBoundaries& bucket_boundaries); + + void Observe(double value) override; + + std::map CountsByBucket(); + + double Sum(); + + double CumulativeCount(); + + cartographer_ros_msgs::msg::Metric ToRosMessage(); + + private: + absl::Mutex mutex_; + const std::map labels_; + const BucketBoundaries bucket_boundaries_; + std::vector bucket_counts_ GUARDED_BY(mutex_); + double sum_ GUARDED_BY(mutex_); +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/include/cartographer_ros/msg_conversion.h similarity index 84% rename from cartographer_ros/cartographer_ros/msg_conversion.h rename to cartographer_ros/include/cartographer_ros/msg_conversion.h index 986322ba1..e895144b2 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/include/cartographer_ros/msg_conversion.h @@ -17,28 +17,21 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H -#include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/io/submap_painter.h" #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros_msgs/msg/landmark_entry.hpp" #include "cartographer_ros_msgs/msg/landmark_list.hpp" -#include -#include -#include -#include -#include -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include -#include -#include -#include - -#include +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include namespace cartographer_ros { @@ -67,7 +60,7 @@ ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& message); +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg); ::cartographer::sensor::LandmarkData ToLandmarkData( const cartographer_ros_msgs::msg::LandmarkList& landmark_list); @@ -95,7 +88,7 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const builtin_interfaces::msg::Time& time); + const rclcpp::Time& time); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/include/cartographer_ros/node.h similarity index 66% rename from cartographer_ros/cartographer_ros/node.h rename to cartographer_ros/include/cartographer_ros/node.h index 70d63909f..f3527cae1 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/include/cartographer_ros/node.h @@ -24,24 +24,25 @@ #include #include +#include "absl/synchronization/mutex.h" #include "cartographer/common/fixed_ratio_sampler.h" -#include "cartographer/common/mutex.h" #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_extrapolator.h" #include "cartographer_ros/map_builder_bridge.h" +#include "cartographer_ros/metrics/family_factory.h" #include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/trajectory_options.h" #include "cartographer_ros_msgs/srv/finish_trajectory.hpp" -#include "cartographer_ros_msgs/msg/sensor_topics.hpp" +#include "cartographer_ros_msgs/srv/get_trajectory_states.hpp" +#include "cartographer_ros_msgs/srv/read_metrics.hpp" #include "cartographer_ros_msgs/srv/start_trajectory.hpp" -#include "cartographer_ros_msgs/msg/status_response.h" +#include "cartographer_ros_msgs/msg/status_response.hpp" #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" -#include "cartographer_ros_msgs/msg/trajectory_options.hpp" #include "cartographer_ros_msgs/srv/write_state.hpp" -#include +#include "nav_msgs/msg/odometry.hpp" #include #include #include @@ -49,17 +50,23 @@ #include #include #include +#include #include -namespace cartographer_ros -{ +namespace cartographer_ros { + // Wires up ROS topics to SLAM. -class Cartographer : public rclcpp::Node -{ +class Node { public: - Cartographer(const NodeOptions& node_options, - std::unique_ptr map_builder); - ~Cartographer(); + Node(const NodeOptions& node_options, + std::unique_ptr map_builder, + std::shared_ptr tf_buffer, + rclcpp::Node::SharedPtr node, + bool collect_metrics); + ~Node(); + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; // Finishes all yet active trajectories. void FinishAllTrajectories(); @@ -90,30 +97,29 @@ class Cartographer : public rclcpp::Node // The following functions handle adding sensor data to a trajectory. void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, - const nav_msgs::msg::Odometry::ConstSharedPtr msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg); void HandleLandmarkMessage( int trajectory_id, const std::string& sensor_id, - const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr msg); + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::Imu::ConstSharedPtr msg); + const sensor_msgs::msg::Imu::ConstSharedPtr &msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::LaserScan::ConstSharedPtr msg); + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); void HandleMultiEchoLaserScanMessage( int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr msg); + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg); void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); // Serializes the complete Node state. - void SerializeState(const std::string& filename); + void SerializeState(const std::string& filename, + const bool include_unfinished_submaps); // Loads a serialized SLAM state from a .pbstream file. void LoadState(const std::string& state_filename, bool load_frozen_state); - rclcpp::Node::SharedPtr node_handle(); - private: struct Subscriber { rclcpp::SubscriptionBase::SharedPtr subscriber; @@ -125,69 +131,75 @@ class Cartographer : public rclcpp::Node std::string topic; }; - void HandleSubmapQuery( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - void HandleStartTrajectory( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - void HandleFinishTrajectory( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - void HandleWriteState( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool handleSubmapQuery( + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response); + bool handleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response); + bool handleStartTrajectory( + const cartographer_ros_msgs::srv::StartTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::StartTrajectory::Response::SharedPtr response); + bool handleFinishTrajectory( + const cartographer_ros_msgs::srv::FinishTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::FinishTrajectory::Response::SharedPtr response); + bool handleWriteState( + const cartographer_ros_msgs::srv::WriteState::Request::SharedPtr request, + cartographer_ros_msgs::srv::WriteState::Response::SharedPtr response); + bool handleGetTrajectoryStates( + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr, + cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response); + bool handleReadMetrics(const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr, + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response); + // Returns the set of SensorIds expected for a trajectory. // 'SensorId::id' is the expected ROS topic name. std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> - ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics) const; - int AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics); - void LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::msg::SensorTopics& topics, - int trajectory_id); + ComputeExpectedSensorIds(const TrajectoryOptions& options) const; + int AddTrajectory(const TrajectoryOptions& options); + void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); void PublishSubmapList(); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); - void PublishTrajectoryStates(); + void PublishLocalTrajectoryData(); void PublishTrajectoryNodeList(); void PublishLandmarkPosesList(); void PublishConstraintList(); - void SpinOccupancyGridThreadForever(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); - bool ValidateTopicNames(const ::cartographer_ros_msgs::msg::SensorTopics& topics, - const TrajectoryOptions& options); + bool ValidateTopicNames(const TrajectoryOptions& options); cartographer_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock( - int trajectory_id) REQUIRES(mutex_); + int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); + void MaybeWarnAboutTopicMismatch(); + // Helper function for service handlers that need to check trajectory states. + cartographer_ros_msgs::msg::StatusResponse TrajectoryStateToStatus( + int trajectory_id, + const std::set< + cartographer::mapping::PoseGraphInterface::TrajectoryState>& + valid_states); const NodeOptions node_options_; std::shared_ptr tf_broadcaster_; - std::shared_ptr tf_listener_; - std::shared_ptr tf_buffer_; - cartographer::common::Mutex mutex_; + absl::Mutex mutex_; + std::unique_ptr metrics_registry_; std::shared_ptr map_builder_bridge_ GUARDED_BY(mutex_); - ::rclcpp::Node::SharedPtr node_handle_; + rclcpp::Node::SharedPtr node_; ::rclcpp::Publisher<::cartographer_ros_msgs::msg::SubmapList>::SharedPtr submap_list_publisher_; ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_node_list_publisher_; ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr landmark_poses_list_publisher_; ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr constraint_list_publisher_; + ::rclcpp::Publisher::SharedPtr tracked_pose_publisher_; ::rclcpp::Publisher::SharedPtr scan_matched_point_cloud_publisher_; - + // These ros service servers need to live for the lifetime of the node. ::rclcpp::Service::SharedPtr submap_query_server_; + ::rclcpp::Service::SharedPtr trajectory_query_server; ::rclcpp::Service::SharedPtr start_trajectory_server_; ::rclcpp::Service::SharedPtr finish_trajectory_server_; ::rclcpp::Service::SharedPtr write_state_server_; - -// std::vector<::rclcpp::ServiceBase::SharedPtr> service_servers_; + ::rclcpp::Service::SharedPtr get_trajectory_states_server_; + ::rclcpp::Service::SharedPtr read_metrics_server_; struct TrajectorySensorSamplers { @@ -211,20 +223,22 @@ class Cartographer : public rclcpp::Node // These are keyed with 'trajectory_id'. std::map extrapolators_; + std::map last_published_tf_stamps_; std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; - std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_); + std::unordered_set trajectories_scheduled_for_finish_; - // We have to keep the timer handles of ::rclcpp::TimerBase around, otherwise - // they do not fire. -// std::vector<::rclcpp::TimerBase::SharedPtr> wall_timers_; + // The timer for publishing local trajectory data (i.e. pose transforms and + // range data point clouds) is a regular timer which is not triggered when + // simulation time is standing still. This prevents overflowing the transform + // listener buffer by publishing the same transforms over and over again. ::rclcpp::TimerBase::SharedPtr submap_list_timer_; - ::rclcpp::TimerBase::SharedPtr trajectory_states_timer_; + ::rclcpp::TimerBase::SharedPtr local_trajectory_data_timer_; ::rclcpp::TimerBase::SharedPtr trajectory_node_list_timer_; ::rclcpp::TimerBase::SharedPtr landmark_pose_list_timer_; ::rclcpp::TimerBase::SharedPtr constrain_list_timer_; - + ::rclcpp::TimerBase::SharedPtr maybe_warn_about_topic_mismatch_timer_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/include/cartographer_ros/node_constants.h similarity index 87% rename from cartographer_ros/cartographer_ros/node_constants.h rename to cartographer_ros/include/cartographer_ros/node_constants.h index 10655195c..07a85f379 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/include/cartographer_ros/node_constants.h @@ -34,13 +34,18 @@ constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kTrackedPoseTopic[] = "tracked_pose"; constexpr char kSubmapQueryServiceName[] = "submap_query"; +constexpr char kTrajectoryQueryServiceName[] = "trajectory_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kWriteStateServiceName[] = "write_state"; +constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states"; +constexpr char kReadMetricsServiceName[] = "read_metrics"; constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list"; constexpr char kConstraintListTopic[] = "constraint_list"; constexpr double kConstraintPublishPeriodSec = 0.5; +constexpr double kTopicMismatchCheckDelaySec = 3.0; constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kLatestOnlyPublisherQueueSize = 1; diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/include/cartographer_ros/node_options.h similarity index 94% rename from cartographer_ros/cartographer_ros/node_options.h rename to cartographer_ros/include/cartographer_ros/node_options.h index 905bc0ac5..95bb15990 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/include/cartographer_ros/node_options.h @@ -25,8 +25,6 @@ #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer_ros/trajectory_options.h" -#include - namespace cartographer_ros { // Top-level options of Cartographer's ROS integration. @@ -37,6 +35,9 @@ struct NodeOptions { double submap_publish_period_sec; double pose_publish_period_sec; double trajectory_publish_period_sec; + bool publish_to_tf = true; + bool publish_tracked_pose = false; + bool use_pose_extrapolator = true; }; NodeOptions CreateNodeOptions( @@ -45,7 +46,6 @@ NodeOptions CreateNodeOptions( std::tuple LoadOptions( const std::string& configuration_directory, const std::string& configuration_basename); - } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H diff --git a/cartographer_ros/cartographer_ros/offline_node.h b/cartographer_ros/include/cartographer_ros/offline_node.h similarity index 92% rename from cartographer_ros/cartographer_ros/offline_node.h rename to cartographer_ros/include/cartographer_ros/offline_node.h index 25f4f5416..1f9f9ace1 100644 --- a/cartographer_ros/cartographer_ros/offline_node.h +++ b/cartographer_ros/include/cartographer_ros/offline_node.h @@ -22,6 +22,7 @@ #include #include +#include #include "cartographer/mapping/map_builder_interface.h" #include "cartographer_ros/node_options.h" @@ -31,7 +32,8 @@ using MapBuilderFactory = std::function( const ::cartographer::mapping::proto::MapBuilderOptions&)>; -void RunOfflineNode(const MapBuilderFactory& map_builder_factory); +void RunOfflineNode(const MapBuilderFactory& map_builder_factory, + rclcpp::Node::SharedPtr cartographer_offline_node); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/include/cartographer_ros/playable_bag.h similarity index 55% rename from cartographer_ros/cartographer_ros/playable_bag.h rename to cartographer_ros/include/cartographer_ros/playable_bag.h index c53f96321..6cdf2df60 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/include/cartographer_ros/playable_bag.h @@ -16,11 +16,16 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H + #include #include -#include "rosbag/bag.h" -#include "rosbag/view.h" +#include "cartographer_ros_msgs/msg/bagfile_progress.hpp" +#include +#include +#include +#include +#include #include "tf2_ros/buffer.h" namespace cartographer_ros { @@ -31,53 +36,58 @@ class PlayableBag { // Returns a boolean indicating whether the message should enter the buffer. using FilteringEarlyMessageHandler = std::function; + std::shared_ptr)>; - PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time, - ros::Time end_time, ros::Duration buffer_delay, + PlayableBag(const std::string& bag_filename, int bag_id, + rclcpp::Duration buffer_delay, FilteringEarlyMessageHandler filtering_early_message_handler); - ros::Time PeekMessageTime() const; - rosbag::MessageInstance GetNextMessage(); + rclcpp::Time PeekMessageTime() const; + rosbag2_storage::SerializedBagMessage GetNextMessage( + cartographer_ros_msgs::msg::BagfileProgress* progress); bool IsMessageAvailable() const; - std::tuple GetBeginEndTime() const; + std::tuple GetBeginEndTime() const; int bag_id() const; + std::set topics() const { return topics_; } + double duration_in_seconds() const { return duration_in_seconds_; } + bool finished() const { return finished_; } + rosbag2_storage::BagMetadata bag_metadata; private: void AdvanceOneMessage(); void AdvanceUntilMessageAvailable(); - std::unique_ptr bag_; - std::unique_ptr view_; - rosbag::View::const_iterator view_iterator_; + std::unique_ptr bag_reader_; bool finished_; const int bag_id_; const std::string bag_filename_; - const double duration_in_seconds_; - int log_counter_; - std::deque buffered_messages_; - const ::ros::Duration buffer_delay_; + double duration_in_seconds_; + int message_counter_; + std::deque buffered_messages_; + const rclcpp::Duration buffer_delay_; FilteringEarlyMessageHandler filtering_early_message_handler_; + std::set topics_; }; class PlayableBagMultiplexer { public: + PlayableBagMultiplexer(rclcpp::Node::SharedPtr node); void AddPlayableBag(PlayableBag playable_bag); // Returns the next message from the multiplexed (merge-sorted) message // stream, along with the bag id corresponding to the message, and whether // this was the last message in that bag. - std::tuple - GetNextMessage(); + std::tuple GetNextMessage(); bool IsMessageAvailable() const; - ros::Time PeekMessageTime() const; + rclcpp::Time PeekMessageTime() const; + + std::set topics() const { return topics_; } private: struct BagMessageItem { - ros::Time message_timestamp; + rclcpp::Time message_timestamp; int bag_index; struct TimestampIsGreater { bool operator()(const BagMessageItem& l, const BagMessageItem& r) { @@ -86,10 +96,19 @@ class PlayableBagMultiplexer { }; }; + rclcpp::Node::SharedPtr node_; + // Publishes information about the bag-file(s) processing and its progress + rclcpp::Publisher::SharedPtr bag_progress_pub_; + // Map between bagfile id and the last time when its progress was published + std::map bag_progress_time_map_; + // The time interval of publishing bag-file(s) processing in seconds + double progress_pub_interval_; + std::vector playable_bags_; std::priority_queue, BagMessageItem::TimestampIsGreater> next_message_queue_; + std::set topics_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.h b/cartographer_ros/include/cartographer_ros/ros_log_sink.h similarity index 93% rename from cartographer_ros/cartographer_ros/ros_log_sink.h rename to cartographer_ros/include/cartographer_ros/ros_log_sink.h index 28538bf67..e603727ca 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.h +++ b/cartographer_ros/include/cartographer_ros/ros_log_sink.h @@ -20,6 +20,7 @@ #include #include "glog/logging.h" +#include namespace cartographer_ros { @@ -38,6 +39,7 @@ class ScopedRosLogSink : public ::google::LogSink { private: bool will_die_; + rclcpp::Logger logger_{rclcpp::get_logger("cartographer logger")}; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_map.h b/cartographer_ros/include/cartographer_ros/ros_map.h similarity index 100% rename from cartographer_ros/cartographer_ros/ros_map.h rename to cartographer_ros/include/cartographer_ros/ros_map.h diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h similarity index 92% rename from cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h rename to cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h index 04dca753f..f2f4f5bcd 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h +++ b/cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h @@ -22,7 +22,8 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/value_conversion_tables.h" namespace cartographer_ros { @@ -59,6 +60,7 @@ class RosMapWritingPointsProcessor ::cartographer::io::FileWriterFactory file_writer_factory_; ::cartographer::mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_; + ::cartographer::mapping::ValueConversionTables conversion_tables_; ::cartographer::mapping::ProbabilityGrid probability_grid_; }; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/include/cartographer_ros/sensor_bridge.h similarity index 90% rename from cartographer_ros/cartographer_ros/sensor_bridge.h rename to cartographer_ros/include/cartographer_ros/sensor_bridge.h index f0b91b087..e53a0c9c4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/include/cartographer_ros/sensor_bridge.h @@ -19,14 +19,13 @@ #include -#include "cartographer/common/optional.h" +#include "absl/types/optional.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros_msgs/msg/landmark_list.hpp" #include #include @@ -36,7 +35,6 @@ #include #include #include -#include #include namespace cartographer_ros { @@ -87,12 +85,13 @@ class SensorBridge { const ::cartographer::sensor::TimedPointCloud& ranges); const int num_subdivisions_per_laser_scan_; - std::map sensor_to_previous_subdivision_time_; + std::map + sensor_to_previous_subdivision_time_; const TfBridge tf_bridge_; - ::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_; + ::cartographer::mapping::TrajectoryBuilderInterface* const + trajectory_builder_; - ::cartographer::common::optional<::cartographer::transform::Rigid3d> - ecef_to_local_frame_; + absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/include/cartographer_ros/submap.h similarity index 81% rename from cartographer_ros/cartographer_ros/submap.h rename to cartographer_ros/include/cartographer_ros/submap.h index fa3bdb50c..48c3a425b 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/include/cartographer_ros/submap.h @@ -17,7 +17,6 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H -#include #include #include #include @@ -26,16 +25,18 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -// #include "ros/ros.h" #include "cartographer_ros_msgs/srv/submap_query.hpp" -#include "rclcpp/rclcpp.hpp" +#include namespace cartographer_ros { // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' // on error. std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( - std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Response> result); + const ::cartographer::mapping::SubmapId& submap_id, + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/tf_bridge.h b/cartographer_ros/include/cartographer_ros/tf_bridge.h similarity index 96% rename from cartographer_ros/cartographer_ros/tf_bridge.h rename to cartographer_ros/include/cartographer_ros/tf_bridge.h index d65f18983..9308790b1 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.h +++ b/cartographer_ros/include/cartographer_ros/tf_bridge.h @@ -20,9 +20,9 @@ #include #include "cartographer/transform/rigid_transform.h" -#include "tf2_ros/buffer.h" - #include "cartographer_ros/time_conversion.h" +#include +#include namespace cartographer_ros { diff --git a/cartographer_ros/cartographer_ros/time_conversion.h b/cartographer_ros/include/cartographer_ros/time_conversion.h similarity index 86% rename from cartographer_ros/cartographer_ros/time_conversion.h rename to cartographer_ros/include/cartographer_ros/time_conversion.h index 9ae1d0b6d..5555f4d49 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.h +++ b/cartographer_ros/include/cartographer_ros/time_conversion.h @@ -19,12 +19,13 @@ #include "cartographer/common/time.h" #include +#include namespace cartographer_ros { -builtin_interfaces::msg::Time ToRos(::cartographer::common::Time time); +rclcpp::Time ToRos(::cartographer::common::Time time); -::cartographer::common::Time FromRos(const builtin_interfaces::msg::Time& time); +::cartographer::common::Time FromRos(const rclcpp::Time& time); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/include/cartographer_ros/trajectory_options.h similarity index 67% rename from cartographer_ros/cartographer_ros/trajectory_options.h rename to cartographer_ros/include/cartographer_ros/trajectory_options.h index 024e84985..93817b7d0 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/include/cartographer_ros/trajectory_options.h @@ -22,9 +22,6 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include - -#include namespace cartographer_ros { @@ -50,27 +47,9 @@ struct TrajectoryOptions { double landmarks_sampling_ratio; }; -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::rclcpp::Time node_time); - TrajectoryOptions CreateTrajectoryOptions( ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose, - ::rclcpp::Time node_time); - -// Try to convert 'msg' into 'options'. Returns false on failure. -bool FromRosMessage(const cartographer_ros_msgs::msg::TrajectoryOptions& msg, - TrajectoryOptions* options); - -// Converts 'trajectory_options' into a ROS message. -cartographer_ros_msgs::msg::TrajectoryOptions ToRosMessage( - const TrajectoryOptions& trajectory_options); - } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/include/cartographer_ros/urdf_reader.h similarity index 85% rename from cartographer_ros/cartographer_ros/urdf_reader.h rename to cartographer_ros/include/cartographer_ros/urdf_reader.h index 93d8d11f6..ab46bfa6c 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.h +++ b/cartographer_ros/include/cartographer_ros/urdf_reader.h @@ -24,8 +24,8 @@ namespace cartographer_ros { -std::vector ReadStaticTransformsFromUrdf( - const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer); +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, std::shared_ptr tf_buffer); } // namespace cartographer_ros diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch b/cartographer_ros/launch/assets_writer_backpack_2d.launch deleted file mode 100644 index 939b6d730..000000000 --- a/cartographer_ros/launch/assets_writer_backpack_2d.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch.py b/cartographer_ros/launch/assets_writer_backpack_2d.launch.py new file mode 100644 index 000000000..ae9eecdab --- /dev/null +++ b/cartographer_ros/launch/assets_writer_backpack_2d.launch.py @@ -0,0 +1,58 @@ +""" + Copyright 2016 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + config_file_arg = DeclareLaunchArgument('config_file', default_value = 'assets_writer_backpack_2d.lua') + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', LaunchConfiguration('configuration_directory'), + '-configuration_basename', LaunchConfiguration('config_file'), + '-urdf_filename', LaunchConfiguration('urdf_filename'), + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + configuration_directory_arg, + config_file_arg, + urdf_filename_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch b/cartographer_ros/launch/assets_writer_backpack_3d.launch deleted file mode 100644 index 2b3d074ca..000000000 --- a/cartographer_ros/launch/assets_writer_backpack_3d.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch.py b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py new file mode 100644 index 000000000..9ed464683 --- /dev/null +++ b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py @@ -0,0 +1,58 @@ +""" + Copyright 2016 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + config_file_arg = DeclareLaunchArgument('config_file', default_value = 'assets_writer_backpack_3d.lua') + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', LaunchConfiguration('configuration_directory'), + '-configuration_basename', LaunchConfiguration('config_file'), + '-urdf_filename', LaunchConfiguration('urdf_filename'), + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + configuration_directory_arg, + config_file_arg, + urdf_filename_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/assets_writer_ros_map.launch b/cartographer_ros/launch/assets_writer_ros_map.launch deleted file mode 100644 index a5c4dca92..000000000 --- a/cartographer_ros/launch/assets_writer_ros_map.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/assets_writer_ros_map.launch.py b/cartographer_ros/launch/assets_writer_ros_map.launch.py new file mode 100644 index 000000000..a2c7e2b5c --- /dev/null +++ b/cartographer_ros/launch/assets_writer_ros_map.launch.py @@ -0,0 +1,54 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'assets_writer_ros_map.lua', + '-urdf_filename', LaunchConfiguration('urdf_filename'), + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + urdf_filename_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/backpack_2d.launch b/cartographer_ros/launch/backpack_2d.launch deleted file mode 100644 index e016df4e4..000000000 --- a/cartographer_ros/launch/backpack_2d.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/backpack_2d.launch.py b/cartographer_ros/launch/backpack_2d.launch.py new file mode 100644 index 000000000..7f6f46e8d --- /dev/null +++ b/cartographer_ros/launch/backpack_2d.launch.py @@ -0,0 +1,75 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': LaunchConfiguration('use_sim_time')}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_2d.lua'], + remappings = [ + ('echoes', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch deleted file mode 100644 index 52dea599c..000000000 --- a/cartographer_ros/launch/backpack_3d.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/backpack_3d.launch.py b/cartographer_ros/launch/backpack_3d.launch.py new file mode 100644 index 000000000..7adc48207 --- /dev/null +++ b/cartographer_ros/launch/backpack_3d.launch.py @@ -0,0 +1,76 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_3d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': LaunchConfiguration('use_sim_time')}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_3d.lua'], + remappings = [ + ('points2_1', 'horizontal_laser_3d'), + ('points2_2', 'vertical_laser_3d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/launch/demo_backpack_2d.launch b/cartographer_ros/launch/demo_backpack_2d.launch deleted file mode 100644 index 248e14b05..000000000 --- a/cartographer_ros/launch/demo_backpack_2d.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_2d.launch.py b/cartographer_ros/launch/demo_backpack_2d.launch.py new file mode 100644 index 000000000..d1d5cd080 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_2d.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + backpack_2d_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/backpack_2d.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + backpack_2d_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch b/cartographer_ros/launch/demo_backpack_2d_localization.launch deleted file mode 100644 index e91e99f5a..000000000 --- a/cartographer_ros/launch/demo_backpack_2d_localization.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch.py b/cartographer_ros/launch/demo_backpack_2d_localization.launch.py new file mode 100644 index 000000000..be6edd8d2 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_2d_localization.launch.py @@ -0,0 +1,94 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + load_state_filename_arg = DeclareLaunchArgument('load_state_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_2d_localization.lua', + '-load_state_filename', LaunchConfiguration('load_state_filename')], + remappings = [ + ('echoes', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + load_state_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_3d.launch b/cartographer_ros/launch/demo_backpack_3d.launch deleted file mode 100644 index f5bfc3a91..000000000 --- a/cartographer_ros/launch/demo_backpack_3d.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_3d.launch.py b/cartographer_ros/launch/demo_backpack_3d.launch.py new file mode 100644 index 000000000..14594c272 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_3d.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + backpack_3d_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/backpack_3d.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + backpack_3d_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_3d_localization.launch b/cartographer_ros/launch/demo_backpack_3d_localization.launch deleted file mode 100644 index 94304a1bc..000000000 --- a/cartographer_ros/launch/demo_backpack_3d_localization.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_3d_localization.launch.py b/cartographer_ros/launch/demo_backpack_3d_localization.launch.py new file mode 100644 index 000000000..c23ac5e30 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_3d_localization.launch.py @@ -0,0 +1,95 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + load_state_filename_arg = DeclareLaunchArgument('load_state_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_3d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_3d_localization.lua', + '-load_state_filename', LaunchConfiguration('load_state_filename'),], + remappings = [ + ('points2_1', 'horizontal_laser_3d'), + ('points2_2', 'vertical_laser_3d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + load_state_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_revo_lds.launch b/cartographer_ros/launch/demo_revo_lds.launch deleted file mode 100644 index 387664491..000000000 --- a/cartographer_ros/launch/demo_revo_lds.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_revo_lds.launch.py b/cartographer_ros/launch/demo_revo_lds.launch.py new file mode 100644 index 000000000..4f88963f1 --- /dev/null +++ b/cartographer_ros/launch/demo_revo_lds.launch.py @@ -0,0 +1,91 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'revo_lds.lua'], + remappings = [ + ('scan', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_taurob_tracker.launch b/cartographer_ros/launch/demo_taurob_tracker.launch deleted file mode 100644 index 1af7f6583..000000000 --- a/cartographer_ros/launch/demo_taurob_tracker.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_taurob_tracker.launch.py b/cartographer_ros/launch/demo_taurob_tracker.launch.py new file mode 100644 index 000000000..bfd16f5da --- /dev/null +++ b/cartographer_ros/launch/demo_taurob_tracker.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + taurob_tracker_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/taurob_tracker.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + taurob_tracker_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/grpc_demo_backpack_2d.launch b/cartographer_ros/launch/grpc_demo_backpack_2d.launch index b8bf58369..9f942a75c 100644 --- a/cartographer_ros/launch/grpc_demo_backpack_2d.launch +++ b/cartographer_ros/launch/grpc_demo_backpack_2d.launch @@ -31,6 +31,7 @@ diff --git a/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch b/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch index 72be95a48..64bf8788b 100644 --- a/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch +++ b/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch @@ -25,6 +25,7 @@ - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/offline_backpack_2d.launch.py b/cartographer_ros/launch/offline_backpack_2d.launch.py new file mode 100644 index 000000000..675df16ea --- /dev/null +++ b/cartographer_ros/launch/offline_backpack_2d.launch.py @@ -0,0 +1,62 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value='false') + rviz_config_arg = DeclareLaunchArgument('rviz_config', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames', default_value = 'backpack_2d.lua') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + + ## ***** Nodes ***** + offline_node_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/offline_node.launch.py'), + launch_arguments = { + 'bag_filenames': LaunchConfiguration('bag_filenames'), + 'no_rviz': LaunchConfiguration('no_rviz'), + 'rviz_config': LaunchConfiguration('rviz_config'), + 'configuration_directory': LaunchConfiguration('configuration_directory'), + 'configuration_basenames': LaunchConfiguration('configuration_basenames'), + 'urdf_filenames': LaunchConfiguration('urdf_filenames')}.items(), + + ) + set_remap = SetRemap('horizontal_laser_2d', 'echoes') + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + set_remap, + offline_node_launch, + ]) diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch deleted file mode 100644 index 214b3c06b..000000000 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/offline_backpack_3d.launch.py b/cartographer_ros/launch/offline_backpack_3d.launch.py new file mode 100644 index 000000000..4e5d2caae --- /dev/null +++ b/cartographer_ros/launch/offline_backpack_3d.launch.py @@ -0,0 +1,64 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value='false') + rviz_config_arg = DeclareLaunchArgument('rviz_config', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames', default_value = 'backpack_3d.lua') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf') + + ## ***** Nodes ***** + offline_node_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/offline_node.launch.py'), + launch_arguments = { + 'bag_filenames': LaunchConfiguration('bag_filenames'), + 'no_rviz': LaunchConfiguration('no_rviz'), + 'rviz_config': LaunchConfiguration('rviz_config'), + 'configuration_directory': LaunchConfiguration('configuration_directory'), + 'configuration_basenames': LaunchConfiguration('configuration_basenames'), + 'urdf_filenames': LaunchConfiguration('urdf_filenames')}.items(), + + ) + set_remap1 = SetRemap('horizontal_laser_3d', 'points2_1') + set_remap2 = SetRemap('vertical_laser_3d', 'points2_2') + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + set_remap1, + set_remap2, + offline_node_launch, + ]) diff --git a/cartographer_ros/launch/offline_mir_100_rviz.launch.py b/cartographer_ros/launch/offline_mir_100_rviz.launch.py new file mode 100644 index 000000000..b8e796c37 --- /dev/null +++ b/cartographer_ros/launch/offline_mir_100_rviz.launch.py @@ -0,0 +1,76 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from os import getenv + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import Shutdown + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filename') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value = 'False') + keep_running_arg = DeclareLaunchArgument('keep_running', default_value = 'False') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + condition = UnlessCondition(LaunchConfiguration('no_rviz')) + ) + + cartographer_offline_node_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_offline_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basenames', 'mir-100-mapping.lua', + '-urdf_filenames', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/mir-100.urdf', + '-use_bag_transforms', 'false', + '-keep_running', LaunchConfiguration('keep_running'), + '-bag_filenames', LaunchConfiguration('bag_filename')], + remappings = [ + ('f_scan', 'scan_1'), + ( 'b_scan', 'scan_2'), + ( 'imu_data','imu'), + ('odom','ignore_odom'), + ('odom_enc','odom')], + output = 'screen' + ) + + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + keep_running_arg, + + # Nodes + rviz_node, + cartographer_offline_node_node, + ]) diff --git a/cartographer_ros/launch/offline_node.launch.py b/cartographer_ros/launch/offline_node.launch.py new file mode 100644 index 000000000..f38649eee --- /dev/null +++ b/cartographer_ros/launch/offline_node.launch.py @@ -0,0 +1,81 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import Shutdown + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz') + rviz_config_arg = DeclareLaunchArgument('rviz_config') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', LaunchConfiguration('rviz_config')], + parameters = [{'use_sim_time': True}], + condition = UnlessCondition(LaunchConfiguration('no_rviz')) + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + cartographer_offline_node_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_offline_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', LaunchConfiguration('configuration_directory'), + '-configuration_basenames', LaunchConfiguration('configuration_basenames'), + '-urdf_filenames', LaunchConfiguration('urdf_filenames'), + '-bag_filenames', LaunchConfiguration('bag_filenames')], + output = 'screen' + ) + + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + rviz_node, + cartographer_occupancy_grid_node, + cartographer_offline_node_node, + ]) diff --git a/cartographer_ros/launch/taurob_tracker.launch b/cartographer_ros/launch/taurob_tracker.launch deleted file mode 100644 index 4111fa094..000000000 --- a/cartographer_ros/launch/taurob_tracker.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/taurob_tracker.launch.py b/cartographer_ros/launch/taurob_tracker.launch.py new file mode 100644 index 000000000..eda55c886 --- /dev/null +++ b/cartographer_ros/launch/taurob_tracker.launch.py @@ -0,0 +1,58 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'taurob_tracker.lua'], + remappings = [ + ('scan', '/spin_laser/scan'), + ('imu', '/imu/data')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': LaunchConfiguration('use_sim_time')}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/launch/visualize_pbstream.launch b/cartographer_ros/launch/visualize_pbstream.launch deleted file mode 100644 index 764f5ff8c..000000000 --- a/cartographer_ros/launch/visualize_pbstream.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - diff --git a/cartographer_ros/launch/visualize_pbstream.launch.py b/cartographer_ros/launch/visualize_pbstream.launch.py new file mode 100644 index 000000000..859c3587a --- /dev/null +++ b/cartographer_ros/launch/visualize_pbstream.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + pbstream_filename_arg = DeclareLaunchArgument('pbstream_filename') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'visualize_pbstream.lua', + '-load_state_filename', LaunchConfiguration('pbstream_filename'), + '-load_frozen_state=false', + '-start_trajectory_with_default_topics=false'], + output = 'screen' + ) + + return LaunchDescription([ + # Launch arguments + pbstream_filename_arg, + # Nodes + rviz_node, + cartographer_node + ]) diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 0a94fb782..1ce505e0c 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -15,71 +15,54 @@ limitations under the License. --> - + cartographer_ros - - - 1.0.9004 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations. This package provides Cartographer's ROS integration. - Chris Lalancette - Pyo + + The Cartographer Authors + Apache 2.0 - https://github.com/ros2/cartographer_ros + + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors - Darby Lim - ament_cmake - eigen - pcl_conversions - tf2_eigen - urdfdom_headers + git + google-mock + protobuf-dev + python3-sphinx cartographer cartographer_ros_msgs - lua5.2-dev - nav_msgs + geometry_msgs + libgflags-dev + libgoogle-glog-dev libpcl-all-dev + builtin_interfaces + rosidl_default_generators + nav_msgs + pcl_conversions + robot_state_publisher + rosbag2 + rosbag2_transport rclcpp + launch sensor_msgs + std_msgs tf2 + tf2_eigen tf2_ros - + urdf visualization_msgs - tf2_msgs - yaml-cpp ament_cmake diff --git a/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py b/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py new file mode 100755 index 000000000..919a748c6 --- /dev/null +++ b/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2018 Magazino GmbH +# The Cartographer Authors +# +# 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. + +import itertools +import random + +import rospy +from tf import transformations +from cartographer_ros_msgs.msg import LandmarkEntry, LandmarkList + +DESC = ''' +Samples a number of random landmarks at a specified rate. +Can be used to test the timing of landmark input or the effect of erroneous +landmarks with duplicate IDs (if --allow_duplicate_ids is set). + +For example: + +./publish_fake_random_landmarks.py \\ + --publish_period 0.1 \\ + --id_vocabulary A B C \\ + --id_length 5 \\ + --sample_period 1.0 + +will publish empty landmark lists at 10Hz and random landmarks every second. +IDs are also sampled, using the cartesian product of the provided "vocabulary". +In the above example, a sampled ID could be e.g. "AACBC" (length=5). +''' + +TOPIC = "landmark" + + +class LandmarkSamplerOptions(object): + + def __init__(self, *args, **kwargs): + self.allow_duplicate_ids = False + self.id_vocabulary = {} + self.id_length = 0 + self.max_distance = 0. + self.num_landmarks = 0 + self.rotation_weight = 0. + self.translation_weight = 0. + for name, arg in kwargs.items(): + setattr(self, name, arg) + + +class LandmarkIdSampler(object): + + def __init__(self, id_vocabulary, id_length): + # Precompute all combinations of the symbols in the vocabulary. + # WARNING: can be huge, check before potentially blocking the system. + if len(id_vocabulary)**id_length > 1e6: + raise ValueError("ID sampling space is too large") + self.sampling_space = list( + itertools.product(*(id_vocabulary for i in range(id_length)))) + + def sample_id(self): + # Draw a random combination of symbols and stringify it. + random_index = random.randint(0, len(self.sampling_space) - 1) + sampled_id = "".join(self.sampling_space[random_index]) + return sampled_id + + +class LandmarkSampler(object): + + def __init__(self, options): + if not isinstance(options, LandmarkSamplerOptions): + raise TypeError("expected LandmarkSamplerOptions") + self.options = options + rospy.loginfo("Initializing landmark ID sampler...") + self.landmark_id_sampler = LandmarkIdSampler(options.id_vocabulary, + options.id_length) + self._sampled_ids = [] + + def random_landmark(self): + landmark = LandmarkEntry() + landmark.translation_weight = self.options.translation_weight + landmark.rotation_weight = self.options.rotation_weight + landmark.id = self.landmark_id_sampler.sample_id() + if landmark.id in self._sampled_ids: + if not self.options.allow_duplicate_ids: + rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id)) + return None + else: + rospy.logwarn("Duplicate ID: {}".format(landmark.id)) + self._sampled_ids.append(landmark.id) + + vector = transformations.random_vector(3) * self.options.max_distance + landmark.tracking_from_landmark_transform.position.x = vector[0] + landmark.tracking_from_landmark_transform.position.y = vector[1] + landmark.tracking_from_landmark_transform.position.z = vector[2] + + quaternion = transformations.random_quaternion() + landmark.tracking_from_landmark_transform.orientation.x = quaternion[0] + landmark.tracking_from_landmark_transform.orientation.y = quaternion[1] + landmark.tracking_from_landmark_transform.orientation.z = quaternion[2] + landmark.tracking_from_landmark_transform.orientation.w = quaternion[3] + return landmark + + def random_landmark_list(self): + landmark_list = LandmarkList() + landmark_list.header.stamp = rospy.Time.now() + for _ in range(self.options.num_landmarks): + random_landmark = self.random_landmark() + if random_landmark is not None: + landmark_list.landmarks.append(random_landmark) + return landmark_list + + +class SampledLandmarkPublisher(object): + + def __init__(self, publish_period, sample_period, landmark_sampler_options): + self.landmark_sampler = LandmarkSampler(landmark_sampler_options) + rospy.loginfo("Publishing landmarks to topic: {}".format(TOPIC)) + self.publisher = rospy.Publisher(TOPIC, LandmarkList, queue_size=1) + self.publish_timer = rospy.Timer( + rospy.Duration(publish_period), self.publish_empty_landmark_list) + self.sample_timer = rospy.Timer( + rospy.Duration(sample_period), self.publish_random_landmark_list) + + def publish_random_landmark_list(self, timer_event): + self.publisher.publish(self.landmark_sampler.random_landmark_list()) + + def publish_empty_landmark_list(self, timer_event): + landmark_list = LandmarkList(rospy.Header(stamp=rospy.Time.now()), []) + self.publisher.publish(landmark_list) + + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser( + description=DESC, formatter_class=argparse.RawTextHelpFormatter) + parser.add_argument("--translation_weight", type=float, default=1e5) + parser.add_argument("--rotation_weight", type=float, default=1e5) + parser.add_argument( + "--publish_period", + type=float, + default=0.1, + help="Baseline period for publishing empty landmark lists.") + parser.add_argument( + "--sample_period", + type=float, + default=5., + help="Period at which randomly sampled landmarks are published.") + parser.add_argument( + "--num_landmarks", + type=int, + default=5, + help="The number of random landmarks published simultaneously.") + parser.add_argument( + "--max_distance", + type=float, + default=1.0, + help="Maximum distance of a random landmark to the tracking frame.") + parser.add_argument( + "--id_vocabulary", + nargs='+', + default={"a", "b", "c", "1", "2", "3"}, + help="Set of symbols that can appear in random landmark IDs.") + parser.add_argument( + "--id_length", + type=int, + default=5, + help="The length of the random landmark IDs (number of symbols).") + parser.add_argument( + "--allow_duplicate_ids", + action="store_true", + help="Publish landmarks with IDs that have already been published.") + + args, unknown = parser.parse_known_args(rospy.myargv()[1:]) + rospy.init_node("landmark_sampler") + + landmark_sampler_options = LandmarkSamplerOptions(**args.__dict__) + sampler = SampledLandmarkPublisher(args.publish_period, args.sample_period, + landmark_sampler_options) + + rospy.spin() diff --git a/cartographer_ros/src/.clang-format b/cartographer_ros/src/.clang-format new file mode 100644 index 000000000..5650f22c0 --- /dev/null +++ b/cartographer_ros/src/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +DerivePointerAlignment: false diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/src/assets_writer.cpp similarity index 61% rename from cartographer_ros/cartographer_ros/assets_writer.cc rename to cartographer_ros/src/assets_writer.cpp index 49b2c2963..1e765659e 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/src/assets_writer.cpp @@ -20,8 +20,8 @@ #include #include +#include "absl/memory/memory.h" #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" @@ -35,17 +35,15 @@ #include "cartographer/transform/transform_interpolation_buffer.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/ros_map_writing_points_processor.h" -#include "cartographer_ros/split_string.h" #include "cartographer_ros/time_conversion.h" #include "cartographer_ros/urdf_reader.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "ros/ros.h" -#include "ros/time.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_msgs/TFMessage.h" +#include "builtin_interfaces/msg/time.hpp" +#include +#include +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/buffer.h" #include "urdf/model.h" @@ -61,8 +59,7 @@ CreatePipelineBuilder( const std::string file_prefix) { const auto file_writer_factory = AssetsWriter::CreateFileWriterFactory(file_prefix); - auto builder = - carto::common::make_unique(); + auto builder = absl::make_unique(); carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory, builder.get()); builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName, @@ -80,13 +77,13 @@ std::unique_ptr LoadLuaDictionary( const std::string& configuration_directory, const std::string& configuration_basename) { auto file_resolver = - carto::common::make_unique( + absl::make_unique( std::vector{configuration_directory}); const std::string code = file_resolver->GetFileContentOrDie(configuration_basename); auto lua_parameter_dictionary = - carto::common::make_unique( + absl::make_unique( code, std::move(file_resolver)); return lua_parameter_dictionary; } @@ -94,36 +91,38 @@ std::unique_ptr LoadLuaDictionary( template std::unique_ptr HandleMessage( const T& message, const std::string& tracking_frame, - const tf2_ros::Buffer& tf_buffer, + const std::shared_ptr tf_buffer, const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer) { - const carto::common::Time start_time = FromRos(message.header.stamp); + const carto::common::Time start_time = FromRos(message->header.stamp); - auto points_batch = carto::common::make_unique(); + auto points_batch = absl::make_unique(); points_batch->start_time = start_time; - points_batch->frame_id = message.header.frame_id; + points_batch->frame_id = message->header.frame_id; carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time point_cloud_time; std::tie(point_cloud, point_cloud_time) = - ToPointCloudWithIntensities(message); + ToPointCloudWithIntensities(*message); CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); for (size_t i = 0; i < point_cloud.points.size(); ++i) { const carto::common::Time time = - point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); + point_cloud_time + + carto::common::FromSeconds(point_cloud.points[i].time); if (!transform_interpolation_buffer.Has(time)) { continue; } const carto::transform::Rigid3d tracking_to_map = transform_interpolation_buffer.Lookup(time); const carto::transform::Rigid3d sensor_to_tracking = - ToRigid3d(tf_buffer.lookupTransform( - tracking_frame, message.header.frame_id, ToRos(time))); + ToRigid3d(tf_buffer->lookupTransform( + tracking_frame, message->header.frame_id, ToRos(time))); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); - points_batch->points.push_back(sensor_to_map * - point_cloud.points[i].head<3>()); + points_batch->points.push_back( + sensor_to_map * + carto::sensor::ToRangefinderPoint(point_cloud.points[i])); points_batch->intensities.push_back(point_cloud.intensities[i]); // We use the last transform for the origin, which is approximately correct. points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); @@ -179,6 +178,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, const std::string tracking_frame = lua_parameter_dictionary->GetString("tracking_frame"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); do { for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size(); ++trajectory_id) { @@ -189,73 +189,96 @@ void AssetsWriter::Run(const std::string& configuration_directory, if (trajectory_proto.node_size() == 0) { continue; } - tf2_ros::Buffer tf_buffer; + + std::shared_ptr tf_buffer = std::make_shared(clock); + tf_buffer->setUsingDedicatedThread(true); + if (!urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); + ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer); } const carto::transform::TransformInterpolationBuffer transform_interpolation_buffer(trajectory_proto); - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); - rosbag::View view(bag); - const ::ros::Time begin_time = view.getBeginTime(); - const double duration_in_seconds = - (view.getEndTime() - begin_time).toSec(); + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + rosbag2_storage::BagMetadata bag_metadata = bag_reader.get_metadata(); + const rclcpp::Time begin_time(bag_metadata.starting_time.time_since_epoch().count()); // We need to keep 'tf_buffer' small because it becomes very inefficient // otherwise. We make sure that tf_messages are published before any data // messages, so that tf lookups always work. - std::deque delayed_messages; + std::deque delayed_messages; // We publish tf messages one second earlier than other messages. Under // the assumption of higher frequency tf this should ensure that tf can // always interpolate. - const ::ros::Duration kDelay(1.); - for (const rosbag::MessageInstance& message : view) { - if (use_bag_transforms && message.isType()) { - auto tf_message = message.instantiate(); - for (const auto& transform : tf_message->transforms) { + const rclcpp::Duration kDelay(1.0,0.0); + auto serializer = rclcpp::Serialization(); + auto laser_scan_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto pcl2_serializer = rclcpp::Serialization(); + while (bag_reader.has_next()) { + auto message = bag_reader.read_next(); + if (use_bag_transforms && (message->topic_name == kTfStaticTopic || message->topic_name == "/tf")) { + tf2_msgs::msg::TFMessage tf_message; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + serializer.deserialize_message(&serialized_msg, &tf_message); + for (const auto& transform : tf_message.transforms) { try { - tf_buffer.setTransform(transform, "unused_authority", - message.getTopic() == kTfStaticTopic); + tf_buffer->setTransform(transform, "unused_authority", + message->topic_name == kTfStaticTopic); } catch (const tf2::TransformException& ex) { LOG(WARNING) << ex.what(); } } } - while (!delayed_messages.empty() && delayed_messages.front().getTime() < - message.getTime() - kDelay) { - const rosbag::MessageInstance& delayed_message = + while (!delayed_messages.empty() && delayed_messages.front().time_stamp < + message->time_stamp - kDelay.nanoseconds()) { + const auto delayed_message = delayed_messages.front(); std::unique_ptr points_batch; - if (delayed_message.isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } else if (delayed_message - .isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } else if (delayed_message.isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } - if (points_batch != nullptr) { - points_batch->trajectory_id = trajectory_id; - pipeline.back()->Process(std::move(points_batch)); + + for (auto topic_info : bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == delayed_message.topic_name){ + rclcpp::SerializedMessage serialized_msg(*delayed_message.serialized_data); + if (topic_info.topic_metadata.type == "sensor_msgs/msg/LaserScan") { + sensor_msgs::msg::LaserScan::SharedPtr laser_scan_msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, laser_scan_msg.get()); + points_batch = HandleMessage( + laser_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/MultiEchoLaserScan") { + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, multi_echo_laser_scan_msg.get()); + points_batch = HandleMessage( + multi_echo_laser_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + else if (topic_info.topic_metadata.type == "sensor_msgs/msg/PointCloud2") { + sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, pcl2_scan_msg.get()); + points_batch = HandleMessage( + pcl2_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + if (points_batch != nullptr) { + points_batch->trajectory_id = trajectory_id; + pipeline.back()->Process(std::move(points_batch)); + } + delayed_messages.pop_front(); + break; + } } - delayed_messages.pop_front(); } - delayed_messages.push_back(message); - LOG_EVERY_N(INFO, 100000) - << "Processed " << (message.getTime() - begin_time).toSec() - << " of " << duration_in_seconds << " bag time seconds..."; + delayed_messages.push_back(*message); + LOG_EVERY_N(INFO, 10000) + << "Processed " << (message->time_stamp - begin_time.nanoseconds())/1e9 + << " of " << bag_metadata.duration.count()/1e9 << " bag time seconds..."; } - bag.close(); } } while (pipeline.back()->Flush() == carto::io::PointsProcessor::FlushResult::kRestartStream); @@ -264,8 +287,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, ::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory( const std::string& file_path) { const auto file_writer_factory = [file_path](const std::string& filename) { - return carto::common::make_unique(file_path + - filename); + return absl::make_unique(file_path + filename); }; return file_writer_factory; } diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/src/assets_writer_main.cpp similarity index 83% rename from cartographer_ros/cartographer_ros/assets_writer_main.cc rename to cartographer_ros/src/assets_writer_main.cpp index d931abf08..eaa8422f5 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/src/assets_writer_main.cpp @@ -15,9 +15,10 @@ */ #include "cartographer_ros/assets_writer.h" -#include "cartographer_ros/split_string.h" #include "gflags/gflags.h" #include "glog/logging.h" +#include +#include DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " @@ -42,9 +43,13 @@ DEFINE_string(output_file_prefix, "", "will be used."); int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + FLAGS_alsologtostderr = true; + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing."; @@ -54,11 +59,19 @@ int main(int argc, char** argv) { CHECK(!FLAGS_pose_graph_filename.empty()) << "-pose_graph_filename is missing."; + std::regex regex(","); + std::vector bag_filenames( + std::sregex_token_iterator( + FLAGS_bag_filenames.begin(), FLAGS_bag_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); + ::cartographer_ros::AssetsWriter asset_writer( FLAGS_pose_graph_filename, - cartographer_ros::SplitString(FLAGS_bag_filenames, ','), + bag_filenames, FLAGS_output_file_prefix); asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename, FLAGS_urdf_filename, FLAGS_use_bag_transforms); + rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp similarity index 63% rename from cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc rename to cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp index 0e33f991f..20ee55d2a 100644 --- a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc +++ b/cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp @@ -14,6 +14,7 @@ * limitations under the License. */ +#include "absl/memory/memory.h" #include "cartographer/cloud/client/map_builder_stub.h" #include "cartographer_ros/node.h" #include "cartographer_ros/node_options.h" @@ -21,6 +22,9 @@ #include "gflags/gflags.h" #include "tf2_ros/transform_listener.h" +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " "second is always the Cartographer installation to allow " @@ -29,8 +33,7 @@ DEFINE_string(configuration_basename, "", "Basename, i.e. not containing any directory prefix, of the " "configuration file."); DEFINE_string(server_address, "localhost:50051", - "gRPC server address to " - "stream the sensor data to."); + "gRPC server address to stream the sensor data to."); DEFINE_bool( start_trajectory_with_default_topics, true, "Enable to immediately start the first trajectory with default topics."); @@ -39,7 +42,16 @@ DEFINE_string( "If non-empty, serialize state and write it to disk before shutting down."); DEFINE_string(load_state_filename, "", "If non-empty, filename of a .pbstream file " - "to load, containing a saved SLAM state."); + "to load, containing a saved SLAM state. " + "Unless --upload_load_state_file is set, the filepath refers " + "to the gRPC server's file system."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_bool(upload_load_state_file, false, + "Upload the .pbstream file from a local path to the (remote) gRPC " + "server instead of loading it from the server file system."); +DEFINE_string(client_id, "", + "Cartographer client ID to use when connecting to the server."); namespace cartographer_ros { namespace { @@ -53,13 +65,19 @@ void Run() { std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); - auto map_builder = - cartographer::common::make_unique<::cartographer::cloud::MapBuilderStub>( - FLAGS_server_address); - Node node(node_options, std::move(map_builder), &tf_buffer); + auto map_builder = absl::make_unique<::cartographer::cloud::MapBuilderStub>( + FLAGS_server_address, FLAGS_client_id); - if (!FLAGS_load_state_filename.empty()) { - node.LoadState(FLAGS_load_state_filename, true /* load_frozen_state */); + if (!FLAGS_load_state_filename.empty() && !FLAGS_upload_load_state_file) { + map_builder->LoadStateFromFile(FLAGS_load_state_filename, + FLAGS_load_frozen_state); + } + + Node node(node_options, std::move(map_builder), &tf_buffer, + FLAGS_collect_metrics); + + if (!FLAGS_load_state_filename.empty() && FLAGS_upload_load_state_file) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { @@ -72,7 +90,8 @@ void Run() { node.RunFinalOptimization(); if (!FLAGS_save_map_filename.empty()) { - node.SerializeState(FLAGS_save_map_filename); + node.SerializeState(FLAGS_save_map_filename, + false /* include_unfinished_submaps */); } } @@ -87,6 +106,7 @@ int main(int argc, char** argv) { << "-configuration_directory is missing."; CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; + CHECK(!FLAGS_client_id.empty()) << "-client_id is missing."; ::ros::init(argc, argv, "cartographer_grpc_node"); ::ros::start(); diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc b/cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp similarity index 82% rename from cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc rename to cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp index ffaabf150..3b8999766 100644 --- a/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc +++ b/cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp @@ -23,11 +23,15 @@ DEFINE_string(server_address, "localhost:50051", "gRPC server address to " "stream the sensor data to."); +DEFINE_string(client_id, "", + "Cartographer client ID to use when connecting to the server."); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_client_id.empty()) << "-client_id is missing."; + ::ros::init(argc, argv, "cartographer_grpc_offline_node"); ::ros::start(); @@ -35,8 +39,8 @@ int main(int argc, char** argv) { const cartographer_ros::MapBuilderFactory map_builder_factory = [](const ::cartographer::mapping::proto::MapBuilderOptions&) { - return ::cartographer::common::make_unique< - ::cartographer::cloud::MapBuilderStub>(FLAGS_server_address); + return absl::make_unique< ::cartographer::cloud::MapBuilderStub>( + FLAGS_server_address, FLAGS_client_id); }; cartographer_ros::RunOfflineNode(map_builder_factory); diff --git a/cartographer_ros/cartographer_ros/configuration_files_test.cc b/cartographer_ros/src/configuration_files_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/configuration_files_test.cc rename to cartographer_ros/src/configuration_files_test.cpp diff --git a/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp b/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp new file mode 100644 index 000000000..27da714ff --- /dev/null +++ b/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp @@ -0,0 +1,102 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * 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. + */ + +#include +#include +#include + +#include "absl/strings/str_cat.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "geometry_msgs/TransformStamped.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "rosbag/bag.h" +#include "tf2_msgs/TFMessage.h" + +DEFINE_string(input, "", "pbstream file to process"); +DEFINE_string(output, "", "Bag file to write to."); +DEFINE_string(parent_frame, "map", "Frame id to use as parent frame."); + +namespace cartographer_ros { +namespace { + +geometry_msgs::msg::TransformStamped ToTransformStamped( + int64_t timestamp_uts, const std::string& parent_frame_id, + const std::string& child_frame_id, + const cartographer::transform::proto::Rigid3d& parent_T_child) { + static int64_t seq = 0; + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.seq = ++seq; + transform_stamped.header.frame_id = parent_frame_id; + transform_stamped.header.stamp = cartographer_ros::ToRos( + ::cartographer::common::FromUniversal(timestamp_uts)); + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = cartographer_ros::ToGeometryMsgTransform( + ::cartographer::transform::ToRigid3(parent_T_child)); + return transform_stamped; +} + +void pbstream_trajectories_to_bag(const std::string& pbstream_filename, + const std::string& output_bag_filename, + const std::string& parent_frame_id) { + const auto pose_graph = + cartographer::io::DeserializePoseGraphFromFile(FLAGS_input); + + rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write); + for (const auto trajectory : pose_graph.trajectory()) { + const auto child_frame_id = + absl::StrCat("trajectory_", trajectory.trajectory_id()); + LOG(INFO) + << "Writing tf and geometry_msgs/TransformStamped for trajectory id " + << trajectory.trajectory_id() << " with " << trajectory.node_size() + << " nodes."; + for (const auto& node : trajectory.node()) { + tf2_msgs::TFMessage tf_msg; + geometry_msgs::msg::TransformStamped transform_stamped = ToTransformStamped( + node.timestamp(), parent_frame_id, child_frame_id, node.pose()); + tf_msg.transforms.push_back(transform_stamped); + bag.write(child_frame_id, transform_stamped.header.stamp, + transform_stamped); + bag.write("/tf", transform_stamped.header.stamp, tf_msg); + } + } +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char* argv[]) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "Extracts all trajectories from the pbstream and creates a bag file with " + "the trajectory poses stored in /tf.\nAdditionally, each trajectory is " + "also written separately to a geometry_msgs/TransformStamped topic named " + "after the TF child_frame_id of the trajectory.\n For each trajectory, " + "the tool will write transforms with the tf parent_frame_id set " + "according to the `parent_frame` commandline flag and child_frame_id to " + "`trajectory_i`, with `i` corresponding to the `trajectory_id`."); + google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_input.empty()) << "-input pbstream is missing."; + CHECK(!FLAGS_output.empty()) << "-output is missing."; + + cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output, + FLAGS_parent_frame); + return 0; +} diff --git a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc b/cartographer_ros/src/dev/rosbag_publisher_main.cpp similarity index 81% rename from cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc rename to cartographer_ros/src/dev/rosbag_publisher_main.cpp index e0694820c..921710716 100644 --- a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc +++ b/cartographer_ros/src/dev/rosbag_publisher_main.cpp @@ -37,7 +37,7 @@ template void PublishWithModifiedTimestamp(MessagePtrType message, const ros::Publisher& publisher, ros::Duration bag_to_current) { - ros::Time& stamp = message->header.stamp; + rclcpp::Time& stamp = message->header.stamp; stamp += bag_to_current; publisher.publish(message); } @@ -47,7 +47,7 @@ void PublishWithModifiedTimestamp( tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher, ros::Duration bag_to_current) { for (const auto& transform : message->transforms) { - ros::Time& stamp = const_cast(transform.header.stamp); + rclcpp::Time& stamp = const_cast(transform.header.stamp); stamp += bag_to_current; } publisher.publish(message); @@ -81,7 +81,7 @@ int main(int argc, char** argv) { node_handle.getParam("/use_sim_time", use_sim_time); if (use_sim_time) { LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting " - "ros::Time and message header times or weird behavior."; + "rclcpp::Time and message header times or weird behavior."; } std::map topic_to_publisher; for (const rosbag::ConnectionInfo* c : view.getConnections()) { @@ -95,35 +95,35 @@ int main(int argc, char** argv) { ros::Duration(1).sleep(); CHECK(ros::ok()); - ros::Time current_start = ros::Time::now(); - ros::Time bag_start = view.getBeginTime(); + rclcpp::Time current_start = rclcpp::Clock().now(); + rclcpp::Time bag_start = view.getBeginTime(); ros::Duration bag_to_current = current_start - bag_start; for (const rosbag::MessageInstance& message : view) { ros::Duration after_bag_start = message.getTime() - bag_start; if (!::ros::ok()) { break; } - ros::Time planned_publish_time = current_start + after_bag_start; - ros::Time::sleepUntil(planned_publish_time); + rclcpp::Time planned_publish_time = current_start + after_bag_start; + rclcpp::Time::sleepUntil(planned_publish_time); ros::Publisher& publisher = topic_to_publisher.at(message.getTopic()); - if (message.isType()) { + if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { + } else if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { + } else if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { - PublishWithModifiedTimestamp(message.instantiate(), + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { - PublishWithModifiedTimestamp(message.instantiate(), + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), publisher, bag_to_current); } else if (message.isType()) { PublishWithModifiedTimestamp(message.instantiate(), @@ -132,7 +132,7 @@ int main(int argc, char** argv) { LOG(WARNING) << "Skipping message with type " << message.getDataType(); } - ros::Time current_time = ros::Time::now(); + rclcpp::Time current_time = rclcpp::Clock().now(); double simulation_delay = cartographer::common::ToSeconds( cartographer_ros::FromRos(current_time) - cartographer_ros::FromRos(planned_publish_time)); diff --git a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc b/cartographer_ros/src/dev/trajectory_comparison_main.cpp similarity index 88% rename from cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc rename to cartographer_ros/src/dev/trajectory_comparison_main.cpp index da37aa601..95b2d8adb 100644 --- a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc +++ b/cartographer_ros/src/dev/trajectory_comparison_main.cpp @@ -32,14 +32,19 @@ #include "ros/time.h" #include "rosbag/bag.h" #include "rosbag/view.h" -#include "tf2_eigen/tf2_eigen.h" +#include "tf2_eigen/tf2_eigen.hpp" #include "tf2_msgs/TFMessage.h" DEFINE_string(bag_filename, "", - "Bags to process, must be in the same order as the trajectories " - "in 'pose_graph_filename'."); + "Bag file containing TF messages of the trajectory that will be " + "compared against the trajectory in the .pbstream file."); +DEFINE_string(tf_parent_frame, "map", + "The parent frame ID of the TF trajectory from the bag file."); +DEFINE_string(tf_child_frame, "base_link", + "The child frame ID of the TF trajectory from the bag file."); DEFINE_string(pbstream_filename, "", - "Proto stream file containing the pose graph."); + "Proto stream file containing the pose graph. The last " + "trajectory will be used for comparison."); namespace cartographer_ros { namespace { @@ -84,8 +89,8 @@ void Run(const std::string& pbstream_filename, } auto tf_message = message.instantiate(); for (const auto& transform : tf_message->transforms) { - if (transform.header.frame_id != "map" || - transform.child_frame_id != "base_link") { + if (transform.header.frame_id != FLAGS_tf_parent_frame || + transform.child_frame_id != FLAGS_tf_child_frame) { continue; } const cartographer::common::Time transform_time = diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/src/map_builder_bridge.cpp similarity index 80% rename from cartographer_ros/cartographer_ros/map_builder_bridge.cc rename to cartographer_ros/src/map_builder_bridge.cpp index de90c8997..4ea2b191d 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/src/map_builder_bridge.cpp @@ -16,11 +16,11 @@ #include "cartographer_ros/map_builder_bridge.h" -#include "cartographer/common/make_unique.h" +#include "absl/memory/memory.h" #include "cartographer/io/color.h" #include "cartographer/io/proto_stream.h" -#include "cartographer/mapping/pose_graph.h" #include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" #include "cartographer_ros_msgs/msg/status_code.hpp" #include "cartographer_ros_msgs/msg/status_response.hpp" @@ -30,7 +30,7 @@ namespace { using ::cartographer::transform::Rigid3d; constexpr double kTrajectoryLineStripMarkerScale = 0.07; -constexpr double kLandmarkMarkerScale = 0.3; +constexpr double kLandmarkMarkerScale = 0.2; constexpr double kConstraintMarkerScale = 0.025; ::std_msgs::msg::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { @@ -43,8 +43,8 @@ ::std_msgs::msg::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) } visualization_msgs::msg::Marker CreateTrajectoryMarker(const int trajectory_id, - const std::string& frame_id, - ::rclcpp::Time node_time) { + const std::string& frame_id, + rclcpp::Time node_time) { visualization_msgs::msg::Marker marker; marker.ns = "Trajectory " + std::to_string(trajectory_id); marker.id = 0; @@ -73,11 +73,11 @@ int GetLandmarkIndex( visualization_msgs::msg::Marker CreateLandmarkMarker(int landmark_index, const Rigid3d& landmark_pose, const std::string& frame_id, - ::rclcpp::Time node_time) { + rclcpp::Time node_time) { visualization_msgs::msg::Marker marker; marker.ns = "Landmarks"; marker.id = landmark_index; - marker.type = visualization_msgs::msg::Marker::CUBE; + marker.type = visualization_msgs::msg::Marker::SPHERE; marker.header.stamp = node_time; marker.header.frame_id = frame_id; marker.scale.x = kLandmarkMarkerScale; @@ -125,20 +125,23 @@ int MapBuilderBridge::AddTrajectory( const TrajectoryOptions& trajectory_options) { const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, - ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, - ::std::placeholders::_1, ::std::placeholders::_2, - ::std::placeholders::_3, ::std::placeholders::_4, - ::std::placeholders::_5)); + [this](const int trajectory_id, const ::cartographer::common::Time time, + const Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local, + const std::unique_ptr< + const ::cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>) { + OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); + }); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; // Make sure there is no trajectory with 'trajectory_id' yet. CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); - sensor_bridges_[trajectory_id] = - cartographer::common::make_unique( - trajectory_options.num_subdivisions_per_laser_scan, - trajectory_options.tracking_frame, - node_options_.lookup_transform_timeout_sec, tf_buffer_, - map_builder_->GetTrajectoryBuilder(trajectory_id)); + sensor_bridges_[trajectory_id] = absl::make_unique( + trajectory_options.num_subdivisions_per_laser_scan, + trajectory_options.tracking_frame, + node_options_.lookup_transform_timeout_sec, tf_buffer_, + map_builder_->GetTrajectoryBuilder(trajectory_id)); auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options); CHECK(emplace_result.second == true); @@ -149,7 +152,7 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'..."; // Make sure there is a trajectory with 'trajectory_id'. - CHECK_EQ(sensor_bridges_.count(trajectory_id), 1); + CHECK(GetTrajectoryStates().count(trajectory_id)); map_builder_->FinishTrajectory(trajectory_id); sensor_bridges_.erase(trajectory_id); } @@ -159,15 +162,15 @@ void MapBuilderBridge::RunFinalOptimization() { map_builder_->pose_graph()->RunFinalOptimization(); } -bool MapBuilderBridge::SerializeState(const std::string& filename) { - cartographer::io::ProtoStreamWriter writer(filename); - map_builder_->SerializeState(&writer); - return writer.Close(); +bool MapBuilderBridge::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { + return map_builder_->SerializeStateToFile(include_unfinished_submaps, + filename); } void MapBuilderBridge::HandleSubmapQuery( - const std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Request> request, - std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Response> response) { + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response) { cartographer::mapping::proto::SubmapQuery::Response response_proto; cartographer::mapping::SubmapId submap_id{request->trajectory_id, request->submap_index}; @@ -196,24 +199,28 @@ void MapBuilderBridge::HandleSubmapQuery( response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; } -std::set MapBuilderBridge::GetFrozenTrajectoryIds() { - std::set frozen_trajectory_ids; - const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); - for (const int trajectory_id : node_poses.trajectory_ids()) { - if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) { - frozen_trajectory_ids.insert(trajectory_id); - } +std::map +MapBuilderBridge::GetTrajectoryStates() { + auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates(); + // Add active trajectories that are not yet in the pose graph, but are e.g. + // waiting for input sensor data and thus already have a sensor bridge. + for (const auto& sensor_bridge : sensor_bridges_) { + trajectory_states.insert(std::make_pair( + sensor_bridge.first, + ::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE)); } - return frozen_trajectory_ids; + return trajectory_states; } -cartographer_ros_msgs::msg::SubmapList MapBuilderBridge::GetSubmapList(::rclcpp::Time node_time) { +cartographer_ros_msgs::msg::SubmapList MapBuilderBridge::GetSubmapList(rclcpp::Time node_time) { cartographer_ros_msgs::msg::SubmapList submap_list; submap_list.header.stamp = node_time; submap_list.header.frame_id = node_options_.map_frame; for (const auto& submap_id_pose : map_builder_->pose_graph()->GetAllSubmapPoses()) { cartographer_ros_msgs::msg::SubmapEntry submap_entry; + submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen( + submap_id_pose.id.trajectory_id); submap_entry.trajectory_id = submap_id_pose.id.trajectory_id; submap_entry.submap_index = submap_id_pose.id.submap_index; submap_entry.submap_version = submap_id_pose.data.version; @@ -223,25 +230,25 @@ cartographer_ros_msgs::msg::SubmapList MapBuilderBridge::GetSubmapList(::rclcpp: return submap_list; } -std::unordered_map -MapBuilderBridge::GetTrajectoryStates() { - std::unordered_map trajectory_states; +std::unordered_map +MapBuilderBridge::GetLocalTrajectoryData() { + std::unordered_map local_trajectory_data; for (const auto& entry : sensor_bridges_) { const int trajectory_id = entry.first; const SensorBridge& sensor_bridge = *entry.second; - std::shared_ptr local_slam_data; + std::shared_ptr local_slam_data; { - cartographer::common::MutexLocker lock(&mutex_); - if (trajectory_state_data_.count(trajectory_id) == 0) { + absl::MutexLock lock(&mutex_); + if (local_slam_data_.count(trajectory_id) == 0) { continue; } - local_slam_data = trajectory_state_data_.at(trajectory_id); + local_slam_data = local_slam_data_.at(trajectory_id); } // Make sure there is a trajectory with 'trajectory_id'. CHECK_EQ(trajectory_options_.count(trajectory_id), 1); - trajectory_states[trajectory_id] = { + local_trajectory_data[trajectory_id] = { local_slam_data, map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id), sensor_bridge.tf_bridge().LookupToTracking( @@ -249,10 +256,36 @@ MapBuilderBridge::GetTrajectoryStates() { trajectory_options_[trajectory_id].published_frame), trajectory_options_[trajectory_id]}; } - return trajectory_states; + return local_trajectory_data; +} + +void MapBuilderBridge::HandleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response) { + // This query is safe if the trajectory doesn't exist (returns 0 poses). + // However, we can filter unwanted states at the higher level in the node. + const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); + for (const auto& node_id_data : + node_poses.trajectory(request->trajectory_id)) { + if (!node_id_data.data.constant_pose_data.has_value()) { + continue; + } + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = node_options_.map_frame; + pose_stamped.header.stamp = + ToRos(node_id_data.data.constant_pose_data.value().time); + pose_stamped.pose = ToGeometryMsgPose(node_id_data.data.global_pose); + response->trajectory.push_back(pose_stamped); + } + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = + "Retrieved " + std::to_string(response->trajectory.size()) + + " trajectory nodes from trajectory " + std::to_string(request->trajectory_id) + "."; } -visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList(::rclcpp::Time node_time) { +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList( + rclcpp::Time node_time) +{ visualization_msgs::msg::MarkerArray trajectory_node_list; const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); // Find the last node indices for each trajectory that have either @@ -268,7 +301,7 @@ visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList(::r const auto constraints = map_builder_->pose_graph()->constraints(); for (const auto& constraint : constraints) { if (constraint.tag == - cartographer::mapping::PoseGraph::Constraint::INTER_SUBMAP) { + cartographer::mapping::PoseGraphInterface::Constraint::INTER_SUBMAP) { if (constraint.node_id.trajectory_id == constraint.submap_id.trajectory_id) { trajectory_to_last_inter_submap_constrained_node[constraint.node_id @@ -352,7 +385,9 @@ visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList(::r return trajectory_node_list; } -visualization_msgs::msg::MarkerArray MapBuilderBridge::GetLandmarkPosesList(::rclcpp::Time node_time) { +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetLandmarkPosesList( + rclcpp::Time node_time) +{ visualization_msgs::msg::MarkerArray landmark_poses_list; const std::map landmark_poses = map_builder_->pose_graph()->GetLandmarkPoses(); @@ -364,7 +399,7 @@ visualization_msgs::msg::MarkerArray MapBuilderBridge::GetLandmarkPosesList(::rc return landmark_poses_list; } -visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(::rclcpp::Time node_time) { +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(rclcpp::Time node_time) { visualization_msgs::msg::MarkerArray constraint_list; int marker_id = 0; visualization_msgs::msg::Marker constraint_intra_marker; @@ -420,7 +455,7 @@ visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(::rclcp visualization_msgs::msg::Marker *constraint_marker, *residual_marker; std_msgs::msg::ColorRGBA color_constraint, color_residual; if (constraint.tag == - cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) { + cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP) { constraint_marker = &constraint_intra_marker; residual_marker = &residual_intra_marker; // Color mapping for submaps of various trajectories - add trajectory id @@ -496,15 +531,13 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { void MapBuilderBridge::OnLocalSlamResult( const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, - ::cartographer::sensor::RangeData range_data_in_local, - const std::unique_ptr) { - std::shared_ptr local_slam_data = - std::make_shared( - TrajectoryState::LocalSlamData{time, local_pose, - std::move(range_data_in_local)}); - cartographer::common::MutexLocker lock(&mutex_); - trajectory_state_data_[trajectory_id] = std::move(local_slam_data); + ::cartographer::sensor::RangeData range_data_in_local) { + std::shared_ptr local_slam_data = + std::make_shared( + LocalTrajectoryData::LocalSlamData{time, local_pose, + std::move(range_data_in_local)}); + absl::MutexLock lock(&mutex_); + local_slam_data_[trajectory_id] = std::move(local_slam_data); } } // namespace cartographer_ros diff --git a/cartographer_ros/src/metrics/family_factory.cpp b/cartographer_ros/src/metrics/family_factory.cpp new file mode 100644 index 000000000..388c226f5 --- /dev/null +++ b/cartographer_ros/src/metrics/family_factory.cpp @@ -0,0 +1,69 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/metrics/family_factory.h" + +#include "absl/memory/memory.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +::cartographer::metrics::Family<::cartographer::metrics::Counter>* +FamilyFactory::NewCounterFamily(const std::string& name, + const std::string& description) { + auto wrapper = absl::make_unique(name, description); + auto* ptr = wrapper.get(); + counter_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Gauge>* +FamilyFactory::NewGaugeFamily(const std::string& name, + const std::string& description) { + auto wrapper = absl::make_unique(name, description); + auto* ptr = wrapper.get(); + gauge_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +::cartographer::metrics::Family<::cartographer::metrics::Histogram>* +FamilyFactory::NewHistogramFamily(const std::string& name, + const std::string& description, + const BucketBoundaries& boundaries) { + auto wrapper = + absl::make_unique(name, description, boundaries); + auto* ptr = wrapper.get(); + histogram_families_.emplace_back(std::move(wrapper)); + return ptr; +} + +void FamilyFactory::ReadMetrics( + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const { + for (const auto& counter_family : counter_families_) { + response->metric_families.push_back(counter_family->ToRosMessage()); + } + for (const auto& gauge_family : gauge_families_) { + response->metric_families.push_back(gauge_family->ToRosMessage()); + } + for (const auto& histogram_family : histogram_families_) { + response->metric_families.push_back(histogram_family->ToRosMessage()); + } +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/cartographer_ros/src/metrics/internal/family.cpp b/cartographer_ros/src/metrics/internal/family.cpp new file mode 100644 index 000000000..29572a9e8 --- /dev/null +++ b/cartographer_ros/src/metrics/internal/family.cpp @@ -0,0 +1,82 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/metrics/internal/family.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +Counter* CounterFamily::Add(const std::map& labels) { + auto wrapper = absl::make_unique(labels); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::msg::MetricFamily CounterFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +Gauge* GaugeFamily::Add(const std::map& labels) { + auto wrapper = absl::make_unique(labels); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::msg::MetricFamily GaugeFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +Histogram* HistogramFamily::Add( + const std::map& labels) { + auto wrapper = absl::make_unique(labels, boundaries_); + auto* ptr = wrapper.get(); + wrappers_.emplace_back(std::move(wrapper)); + return ptr; +} + +cartographer_ros_msgs::msg::MetricFamily HistogramFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; + family_msg.name = name_; + family_msg.description = description_; + for (const auto& wrapper : wrappers_) { + family_msg.metrics.push_back(wrapper->ToRosMessage()); + } + return family_msg; +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/cartographer_ros/src/metrics/internal/histogram.cpp b/cartographer_ros/src/metrics/internal/histogram.cpp new file mode 100644 index 000000000..40fe625a4 --- /dev/null +++ b/cartographer_ros/src/metrics/internal/histogram.cpp @@ -0,0 +1,90 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/metrics/internal/histogram.h" + +#include +#include + +#include "glog/logging.h" + +namespace cartographer_ros { +namespace metrics { + +using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries; + +Histogram::Histogram(const std::map& labels, + const BucketBoundaries& bucket_boundaries) + : labels_(labels), + bucket_boundaries_(bucket_boundaries), + bucket_counts_(bucket_boundaries.size() + 1) { + absl::MutexLock lock(&mutex_); + CHECK(std::is_sorted(std::begin(bucket_boundaries_), + std::end(bucket_boundaries_))); +} + +void Histogram::Observe(double value) { + auto bucket_index = + std::distance(bucket_boundaries_.begin(), + std::upper_bound(bucket_boundaries_.begin(), + bucket_boundaries_.end(), value)); + absl::MutexLock lock(&mutex_); + sum_ += value; + bucket_counts_[bucket_index] += 1; +} + +std::map Histogram::CountsByBucket() { + absl::MutexLock lock(&mutex_); + std::map counts_by_bucket; + // Add the finite buckets. + for (size_t i = 0; i < bucket_boundaries_.size(); ++i) { + counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i); + } + // Add the "infinite" bucket. + counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back(); + return counts_by_bucket; +} + +double Histogram::Sum() { + absl::MutexLock lock(&mutex_); + return sum_; +} + +double Histogram::CumulativeCount() { + absl::MutexLock lock(&mutex_); + return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.); +} + +cartographer_ros_msgs::msg::Metric Histogram::ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg; + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_HISTOGRAM; + for (const auto& label : labels_) { + cartographer_ros_msgs::msg::MetricLabel label_msg; + label_msg.key = label.first; + label_msg.value = label.second; + msg.labels.push_back(label_msg); + } + for (const auto& bucket : CountsByBucket()) { + cartographer_ros_msgs::msg::HistogramBucket bucket_msg; + bucket_msg.bucket_boundary = bucket.first; + bucket_msg.count = bucket.second; + msg.counts_by_bucket.push_back(bucket_msg); + } + return msg; +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/cartographer_ros/src/metrics/internal/metrics_test.cpp b/cartographer_ros/src/metrics/internal/metrics_test.cpp new file mode 100644 index 000000000..d0c868124 --- /dev/null +++ b/cartographer_ros/src/metrics/internal/metrics_test.cpp @@ -0,0 +1,104 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include +#include +#include + +#include "cartographer/metrics/histogram.h" +#include "cartographer_ros/metrics/internal/counter.h" +#include "cartographer_ros/metrics/internal/gauge.h" +#include "cartographer_ros/metrics/internal/histogram.h" +#include "gtest/gtest.h" + +namespace cartographer_ros { +namespace metrics { + +TEST(Metrics, GaugeTest) { + Gauge gauge({}); + EXPECT_EQ(gauge.Value(), 0.); + gauge.Increment(1.2); + EXPECT_EQ(gauge.Value(), 1.2); + gauge.Increment(); + EXPECT_EQ(gauge.Value(), 2.2); + gauge.Decrement(2.2); + EXPECT_EQ(gauge.Value(), 0.); + gauge.Decrement(); + EXPECT_EQ(gauge.Value(), -1.); +} + +TEST(Metrics, CounterTest) { + Counter counter({}); + EXPECT_EQ(counter.Value(), 0.); + counter.Increment(1.2); + EXPECT_EQ(counter.Value(), 1.2); + counter.Increment(0.8); + EXPECT_EQ(counter.Value(), 2.); + counter.Increment(); + EXPECT_EQ(counter.Value(), 3.); +} + +TEST(Metrics, HistogramFixedWidthTest) { + auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3); + Histogram histogram({}, boundaries); + + // Observe some values that fit in finite buckets. + std::array values = {{0., 2, 2.5}}; + for (const auto& value : values) { + histogram.Observe(value); + } + // 1 2 3 inf + // 1 | 0 | 2 | 0 | + EXPECT_EQ(histogram.CountsByBucket()[1], 1); + EXPECT_EQ(histogram.CountsByBucket()[2], 0); + EXPECT_EQ(histogram.CountsByBucket()[3], 2); + EXPECT_EQ(histogram.CumulativeCount(), values.size()); + EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.)); + + // Values above the last bucket boundary should go to the "infinite" bucket. + histogram.Observe(3.5); + // 1 2 3 inf + // 1 | 0 | 2 | 1 | + EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1); +} + +TEST(Metrics, HistogramScaledPowersOfTest) { + auto boundaries = + ::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048); + Histogram histogram({}, boundaries); + + // Observe some values that fit in finite buckets. + std::array values = {{256, 512, 666}}; + for (const auto& value : values) { + histogram.Observe(value); + } + // ... 256 512 1024 inf + // ... | 1 | 2 | 0 | + EXPECT_EQ(histogram.CountsByBucket()[256], 0); + EXPECT_EQ(histogram.CountsByBucket()[512], 1); + EXPECT_EQ(histogram.CountsByBucket()[1024], 2); + EXPECT_EQ(histogram.CumulativeCount(), values.size()); + EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.)); + + // Values above the last bucket boundary should go to the "infinite" bucket. + histogram.Observe(2048); + // ... 256 512 1024 inf + // ... | 1 | 2 | 1 | + EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1); +} + +} // namespace metrics +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cpp similarity index 71% rename from cartographer_ros/cartographer_ros/msg_conversion.cc rename to cartographer_ros/src/msg_conversion.cpp index 6266b67df..973563ee6 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/src/msg_conversion.cpp @@ -25,26 +25,55 @@ #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/time_conversion.h" -#include -#include -#include -#include -#include -#include +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" #include "glog/logging.h" -#include +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "builtin_interfaces/msg/time.hpp" +//#include "ros/serialization.h" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" -#include -#include -#include -#include -#include +namespace { -namespace cartographer_ros { +// Sizes of PCL point types have to be 4n floats for alignment, as described in +// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php +struct PointXYZT { + float x; + float y; + float z; + float time; +}; + +struct PointXYZIT { + PCL_ADD_POINT4D; + float intensity; + float time; + float unused_padding[2]; +}; + +} // namespace + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, + intensity)(float, time, time)) + +namespace cartographer_ros { namespace { -// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each +// The ros::sensor_msgs::msg::PointCloud2 binary data contains 4 floats for each // point. The last one must be this value or RViz is not showing the point cloud // properly. constexpr float kPointCloudComponentFourMagic = 1.; @@ -57,8 +86,8 @@ using ::cartographer_ros_msgs::msg::LandmarkEntry; using ::cartographer_ros_msgs::msg::LandmarkList; sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, - const std::string& frame_id, - const int num_points) { + const std::string& frame_id, + const int num_points) { sensor_msgs::msg::PointCloud2 msg; msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); msg.header.frame_id = frame_id; @@ -85,12 +114,12 @@ sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, return msg; } -// For sensor_msgs::LaserScan. +// For sensor_msgs::msg::LaserScan. bool HasEcho(float) { return true; } float GetFirstEcho(float range) { return range; } -// For sensor_msgs::MultiEchoLaserScan. +// For sensor_msgs::msg::MultiEchoLaserScan. bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) { return !echo.echoes.empty(); } @@ -99,7 +128,7 @@ float GetFirstEcho(const sensor_msgs::msg::LaserEcho& echo) { return echo.echoes[0]; } -// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan. +// For sensor_msgs::msg::LaserScan and sensor_msgs::msg::MultiEchoLaserScan. template std::tuple LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { @@ -118,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { const float first_echo = GetFirstEcho(echoes); if (msg.range_min <= first_echo && first_echo <= msg.range_max) { const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); - Eigen::Vector4f point; - point << rotation * (first_echo * Eigen::Vector3f::UnitX()), - i * msg.time_increment; + const cartographer::sensor::TimedRangefinderPoint point{ + rotation * (first_echo * Eigen::Vector3f::UnitX()), + i * msg.time_increment}; point_cloud.points.push_back(point); if (msg.intensities.size() > 0) { CHECK_EQ(msg.intensities.size(), msg.ranges.size()); @@ -136,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { } ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back()[3]; + const double duration = point_cloud.points.back().time; timestamp += cartographer::common::FromSeconds(duration); - for (Eigen::Vector4f& point : point_cloud.points) { - point[3] -= duration; + for (auto& point : point_cloud.points) { + point.time -= duration; } } return std::make_tuple(point_cloud, timestamp); @@ -161,16 +190,14 @@ sensor_msgs::msg::PointCloud2 ToPointCloud2Message( const int64_t timestamp, const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& point_cloud) { auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); - size_t offset = 0; float * const data = reinterpret_cast(&msg.data[0]); for (const auto& point : point_cloud) { - data[offset++] = point.x(); - data[offset++] = point.y(); - data[offset++] = point.z(); + data[offset++] = point.position.x(); + data[offset++] = point.position.y(); + data[offset++] = point.position.z(); data[offset++] = kPointCloudComponentFourMagic; } - return msg; } @@ -188,35 +215,74 @@ ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) { std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& message) { +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg) { PointCloudWithIntensities point_cloud; // We check for intensity field here to avoid run-time warnings if we pass in // a PointCloud2 without intensity. - if (PointCloud2HasField(message, "intensity")) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(message, pcl_point_cloud); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); - point_cloud.intensities.push_back(point.intensity); + if (PointCloud2HasField(msg, "intensity")) { + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(point.intensity); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); + point_cloud.intensities.push_back(point.intensity); + } } } else { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(message, pcl_point_cloud); - - // If we don't have an intensity field, just copy XYZ and fill in - // 1.0. - for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); - point_cloud.intensities.push_back(1.0); + // If we don't have an intensity field, just copy XYZ and fill in 1.0f. + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(1.0f); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); + point_cloud.intensities.push_back(1.0f); + } } } - return std::make_tuple(point_cloud, FromRos(message.header.stamp)); + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back().time; + timestamp += cartographer::common::FromSeconds(duration); + for (auto& point : point_cloud.points) { + point.time -= duration; + CHECK_LE(point.time, 0.f) + << "Encountered a point with a larger stamp than " + "the last point in the cloud."; + } + } + return std::make_tuple(point_cloud, timestamp); } LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { LandmarkData landmark_data; landmark_data.time = FromRos(landmark_list.header.stamp); - for (const LandmarkEntry& entry : landmark_list.landmark) { + for (const LandmarkEntry& entry : landmark_list.landmarks) { landmark_data.landmark_observations.push_back( {entry.id, ToRigid3d(entry.tracking_from_landmark_transform), entry.translation_weight, entry.rotation_weight}); @@ -308,14 +374,12 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const builtin_interfaces::msg::Time& time) { - auto occupancy_grid = - ::cartographer::common::make_unique(); + const rclcpp::Time& time) { + auto occupancy_grid = absl::make_unique(); const int width = cairo_image_surface_get_width(painted_slices.surface.get()); const int height = cairo_image_surface_get_height(painted_slices.surface.get()); - // const ::rclcpp::Time now = ros::Time::now(); occupancy_grid->header.stamp = time; occupancy_grid->header.frame_id = frame_id; diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/src/msg_conversion_test.cpp similarity index 84% rename from cartographer_ros/cartographer_ros/msg_conversion_test.cc rename to cartographer_ros/src/msg_conversion_test.cpp index 0f345363f..152fb8084 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/src/msg_conversion_test.cpp @@ -14,11 +14,12 @@ * limitations under the License. */ +#include "cartographer_ros/msg_conversion.h" + #include #include #include "cartographer/transform/rigid_transform_test_helpers.h" -#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -37,7 +38,7 @@ using ::testing::Field; constexpr double kEps = 1e-6; TEST(MsgConversion, LaserScanToPointCloud) { - sensor_msgs::LaserScan laser_scan; + sensor_msgs::msg::LaserScan laser_scan; for (int i = 0; i < 8; ++i) { laser_scan.ranges.push_back(1.f); } @@ -50,29 +51,29 @@ TEST(MsgConversion, LaserScanToPointCloud) { const auto point_cloud = std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; EXPECT_TRUE( - point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[1].isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); + point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[1].position.isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[3].isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); + point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[3].position.isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[5].isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), + point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[5].position.isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[7].isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); + point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[7].position.isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); + for (int i = 0; i < 8; ++i) { + EXPECT_NEAR(point_cloud[i].time, 0.f, kEps); + } } TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { - sensor_msgs::LaserScan laser_scan; + sensor_msgs::msg::LaserScan laser_scan; laser_scan.ranges.push_back(1.f); laser_scan.ranges.push_back(std::numeric_limits::infinity()); laser_scan.ranges.push_back(2.f); @@ -88,9 +89,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; ASSERT_EQ(2, point_cloud.size()); EXPECT_TRUE( - point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), kEps)); + point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); EXPECT_TRUE( - point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps)); + point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps)); + EXPECT_NEAR(point_cloud[0].time, 0.f, kEps); + EXPECT_NEAR(point_cloud[1].time, 0.f, kEps); } ::testing::Matcher EqualsLandmark( @@ -107,10 +110,10 @@ ::testing::Matcher EqualsLandmark( } TEST(MsgConversion, LandmarkListToLandmarkData) { - cartographer_ros_msgs::LandmarkList message; + cartographer_ros_msgs::msg::LandmarkList message; message.header.stamp.fromSec(10); - cartographer_ros_msgs::LandmarkEntry landmark_0; + cartographer_ros_msgs::msg::LandmarkEntry landmark_0; landmark_0.id = "landmark_0"; landmark_0.tracking_from_landmark_transform.position.x = 1.0; landmark_0.tracking_from_landmark_transform.position.y = 2.0; @@ -121,7 +124,7 @@ TEST(MsgConversion, LandmarkListToLandmarkData) { landmark_0.tracking_from_landmark_transform.orientation.z = 0.0; landmark_0.translation_weight = 1.0; landmark_0.rotation_weight = 2.0; - message.landmark.push_back(landmark_0); + message.landmarks.push_back(landmark_0); LandmarkData actual_landmark_data = ToLandmarkData(message); EXPECT_THAT(actual_landmark_data, diff --git a/cartographer_ros/src/node.cpp b/cartographer_ros/src/node.cpp new file mode 100644 index 000000000..813ae4be4 --- /dev/null +++ b/cartographer_ros/src/node.cpp @@ -0,0 +1,918 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/node.h" + +#include +#include +#include + +#include "Eigen/Core" +#include "absl/memory/memory.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/metrics/register.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/metrics/family_factory.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/sensor_bridge.h" +#include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/msg/status_response.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "glog/logging.h" +#include "nav_msgs/msg/odometry.hpp" +//#include "ros/serialization.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace cartographer_ros { + +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + +namespace { +// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and +// calls 'handler' on the 'node' to handle messages. Returns the subscriber. +template +::rclcpp::SubscriptionBase::SharedPtr SubscribeWithHandler( + void (Node::*handler)(int, const std::string&, + const typename MessageType::ConstSharedPtr&), + const int trajectory_id, const std::string& topic, + ::rclcpp::Node::SharedPtr node_handle, Node* const node) { + return node_handle->create_subscription( + topic, rclcpp::SensorDataQoS(), + [node, handler, trajectory_id, topic](const typename MessageType::ConstSharedPtr msg) { + (node->*handler)(trajectory_id, topic, msg); + }); +} + +std::string TrajectoryStateToString(const TrajectoryState trajectory_state) { + switch (trajectory_state) { + case TrajectoryState::ACTIVE: + return "ACTIVE"; + case TrajectoryState::FINISHED: + return "FINISHED"; + case TrajectoryState::FROZEN: + return "FROZEN"; + case TrajectoryState::DELETED: + return "DELETED"; + } + return ""; +} + +} // namespace + +Node::Node( + const NodeOptions& node_options, + std::unique_ptr map_builder, + std::shared_ptr tf_buffer, + rclcpp::Node::SharedPtr node, + const bool collect_metrics) + : node_options_(node_options) +{ + node_ = node; + tf_broadcaster_ = std::make_shared(node_) ; + map_builder_bridge_.reset(new cartographer_ros::MapBuilderBridge(node_options_, std::move(map_builder), tf_buffer.get())); + + absl::MutexLock lock(&mutex_); + if (collect_metrics) { + metrics_registry_ = absl::make_unique(); + carto::metrics::RegisterAllMetrics(metrics_registry_.get()); + } + + submap_list_publisher_ = + node_->create_publisher<::cartographer_ros_msgs::msg::SubmapList>( + kSubmapListTopic, 10); + trajectory_node_list_publisher_ = + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kTrajectoryNodeListTopic, 10); + landmark_poses_list_publisher_ = + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kLandmarkPosesListTopic, 10); + constraint_list_publisher_ = + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kConstraintListTopic, 10); + if (node_options_.publish_tracked_pose) { + tracked_pose_publisher_ = + node_->create_publisher<::geometry_msgs::msg::PoseStamped>( + kTrackedPoseTopic, 10); + } + + scan_matched_point_cloud_publisher_ = + node_->create_publisher( + kScanMatchedPointCloudTopic, 10); + + submap_query_server_ = node_->create_service( + kSubmapQueryServiceName, + std::bind( + &Node::handleSubmapQuery, this, std::placeholders::_1, std::placeholders::_2)); + trajectory_query_server = node_->create_service( + kTrajectoryQueryServiceName, + std::bind( + &Node::handleTrajectoryQuery, this, std::placeholders::_1, std::placeholders::_2)); + start_trajectory_server_ = node_->create_service( + kStartTrajectoryServiceName, + std::bind( + &Node::handleStartTrajectory, this, std::placeholders::_1, std::placeholders::_2)); + finish_trajectory_server_ = node_->create_service( + kFinishTrajectoryServiceName, + std::bind( + &Node::handleFinishTrajectory, this, std::placeholders::_1, std::placeholders::_2)); + write_state_server_ = node_->create_service( + kWriteStateServiceName, + std::bind( + &Node::handleWriteState, this, std::placeholders::_1, std::placeholders::_2)); + get_trajectory_states_server_ = node_->create_service( + kGetTrajectoryStatesServiceName, + std::bind( + &Node::handleGetTrajectoryStates, this, std::placeholders::_1, std::placeholders::_2)); + read_metrics_server_ = node_->create_service( + kReadMetricsServiceName, + std::bind( + &Node::handleReadMetrics, this, std::placeholders::_1, std::placeholders::_2)); + + + submap_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.submap_publish_period_sec * 1000)), + [this]() { + PublishSubmapList(); + }); + if (node_options_.pose_publish_period_sec > 0) { + local_trajectory_data_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.pose_publish_period_sec * 1000)), + [this]() { + PublishLocalTrajectoryData(); + }); + } + trajectory_node_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), + [this]() { + PublishTrajectoryNodeList(); + }); + landmark_pose_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), + [this]() { + PublishLandmarkPosesList(); + }); + constrain_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(kConstraintPublishPeriodSec * 1000)), + [this]() { + PublishConstraintList(); + }); +} + +Node::~Node() { FinishAllTrajectories(); } + +bool Node::handleSubmapQuery( + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response) { + absl::MutexLock lock(&mutex_); + map_builder_bridge_->HandleSubmapQuery(request, response); + return true; +} + +bool Node::handleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response) { + absl::MutexLock lock(&mutex_); + response->status = TrajectoryStateToStatus( + request->trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FINISHED, + TrajectoryState::FROZEN} /* valid states */); + if (response->status.code != cartographer_ros_msgs::msg::StatusCode::OK) { + LOG(ERROR) << "Can't query trajectory from pose graph: " + << response->status.message; + return true; + } + map_builder_bridge_->HandleTrajectoryQuery(request, response); + return true; +} + +void Node::PublishSubmapList() { + absl::MutexLock lock(&mutex_); + submap_list_publisher_->publish(map_builder_bridge_->GetSubmapList(node_->now())); +} + +void Node::AddExtrapolator(const int trajectory_id, + const TrajectoryOptions& options) { + constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms + CHECK(extrapolators_.count(trajectory_id) == 0); + const double gravity_time_constant = + node_options_.map_builder_options.use_trajectory_builder_3d() + ? options.trajectory_builder_options.trajectory_builder_3d_options() + .imu_gravity_time_constant() + : options.trajectory_builder_options.trajectory_builder_2d_options() + .imu_gravity_time_constant(); + extrapolators_.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple( + ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), + gravity_time_constant)); +} + +void Node::AddSensorSamplers(const int trajectory_id, + const TrajectoryOptions& options) { + CHECK(sensor_samplers_.count(trajectory_id) == 0); + sensor_samplers_.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple( + options.rangefinder_sampling_ratio, options.odometry_sampling_ratio, + options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio, + options.landmarks_sampling_ratio)); +} + +void Node::PublishLocalTrajectoryData() { + absl::MutexLock lock(&mutex_); + for (const auto& entry : map_builder_bridge_->GetLocalTrajectoryData()) { + const auto& trajectory_data = entry.second; + + auto& extrapolator = extrapolators_.at(entry.first); + // We only publish a point cloud if it has changed. It is not needed at high + // frequency, and republishing it would be computationally wasteful. + if (trajectory_data.local_slam_data->time != + extrapolator.GetLastPoseTime()) { + if (scan_matched_point_cloud_publisher_->get_subscription_count() > 0) { + // TODO(gaschler): Consider using other message without time + // information. + carto::sensor::TimedPointCloud point_cloud; + point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local + .returns.size()); + for (const cartographer::sensor::RangefinderPoint & point : + trajectory_data.local_slam_data->range_data_in_local.returns) { + point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint( + point, 0.f /* time */)); + } + scan_matched_point_cloud_publisher_->publish(ToPointCloud2Message( + carto::common::ToUniversal(trajectory_data.local_slam_data->time), + node_options_.map_frame, + carto::sensor::TransformTimedPointCloud( + point_cloud, trajectory_data.local_to_map.cast()))); + } + extrapolator.AddPose(trajectory_data.local_slam_data->time, + trajectory_data.local_slam_data->local_pose); + } + + geometry_msgs::msg::TransformStamped stamped_transform; + // If we do not publish a new point cloud, we still allow time of the + // published poses to advance. If we already know a newer pose, we use its + // time instead. Since tf knows how to interpolate, providing newer + // information is better. + const ::cartographer::common::Time now = std::max( + FromRos(node_->now()), extrapolator.GetLastExtrapolatedTime()); + stamped_transform.header.stamp = + node_options_.use_pose_extrapolator + ? ToRos(now) + : ToRos(trajectory_data.local_slam_data->time); + + // Suppress publishing if we already published a transform at this time. + // Due to 2020-07 changes to geometry2, tf buffer will issue warnings for + // repeated transforms with the same timestamp. + if (last_published_tf_stamps_.count(entry.first) && + last_published_tf_stamps_[entry.first] == stamped_transform.header.stamp) + continue; + last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp; + + const Rigid3d tracking_to_local_3d = + node_options_.use_pose_extrapolator + ? extrapolator.ExtrapolatePose(now) + : trajectory_data.local_slam_data->local_pose; + const Rigid3d tracking_to_local = [&] { + if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) { + return carto::transform::Embed3D( + carto::transform::Project2D(tracking_to_local_3d)); + } + return tracking_to_local_3d; + }(); + + const Rigid3d tracking_to_map = + trajectory_data.local_to_map * tracking_to_local; + + if (trajectory_data.published_to_tracking != nullptr) { + if (node_options_.publish_to_tf) { + if (trajectory_data.trajectory_options.provide_odom_frame) { + std::vector stamped_transforms; + + stamped_transform.header.frame_id = node_options_.map_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.odom_frame; + stamped_transform.transform = + ToGeometryMsgTransform(trajectory_data.local_to_map); + stamped_transforms.push_back(stamped_transform); + + stamped_transform.header.frame_id = + trajectory_data.trajectory_options.odom_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_local * (*trajectory_data.published_to_tracking)); + stamped_transforms.push_back(stamped_transform); + + tf_broadcaster_->sendTransform(stamped_transforms); + } else { + stamped_transform.header.frame_id = node_options_.map_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_map * (*trajectory_data.published_to_tracking)); + tf_broadcaster_->sendTransform(stamped_transform); + } + } + if (node_options_.publish_tracked_pose) { + ::geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = node_options_.map_frame; + pose_msg.header.stamp = stamped_transform.header.stamp; + pose_msg.pose = ToGeometryMsgPose(tracking_to_map); + tracked_pose_publisher_->publish(pose_msg); + } + } + } +} + +void Node::PublishTrajectoryNodeList() { + if (trajectory_node_list_publisher_->get_subscription_count() > 0) { + absl::MutexLock lock(&mutex_); + trajectory_node_list_publisher_->publish( + map_builder_bridge_->GetTrajectoryNodeList(node_->now())); + } +} + +void Node::PublishLandmarkPosesList() { + if (landmark_poses_list_publisher_->get_subscription_count() > 0) { + absl::MutexLock lock(&mutex_); + landmark_poses_list_publisher_->publish( + map_builder_bridge_->GetLandmarkPosesList(node_->now())); + } +} + +void Node::PublishConstraintList() { + if (constraint_list_publisher_->get_subscription_count() > 0) { + absl::MutexLock lock(&mutex_); + constraint_list_publisher_->publish(map_builder_bridge_->GetConstraintList(node_->now())); + } +} + +std::set +Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const { + using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + using SensorType = SensorId::SensorType; + std::set expected_topics; + // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. + for (const std::string& topic : + ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { + expected_topics.insert(SensorId{SensorType::RANGE, topic}); + } + for (const std::string& topic : ComputeRepeatedTopicNames( + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { + expected_topics.insert(SensorId{SensorType::RANGE, topic}); + } + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { + expected_topics.insert(SensorId{SensorType::RANGE, topic}); + } + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + if (node_options_.map_builder_options.use_trajectory_builder_3d() || + (node_options_.map_builder_options.use_trajectory_builder_2d() && + options.trajectory_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { + expected_topics.insert(SensorId{SensorType::IMU, kImuTopic}); + } + // Odometry is optional. + if (options.use_odometry) { + expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic}); + } + // NavSatFix is optional. + if (options.use_nav_sat) { + expected_topics.insert( + SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic}); + } + // Landmark is optional. + if (options.use_landmarks) { + expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic}); + } + return expected_topics; +} + +int Node::AddTrajectory(const TrajectoryOptions& options) { + const std::set + expected_sensor_ids = ComputeExpectedSensorIds(options); + const int trajectory_id = + map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); + AddExtrapolator(trajectory_id, options); + AddSensorSamplers(trajectory_id, options); + LaunchSubscribers(options, trajectory_id); + maybe_warn_about_topic_mismatch_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(kTopicMismatchCheckDelaySec * 1000)), + [this]() { + MaybeWarnAboutTopicMismatch(); + }); + for (const auto& sensor_id : expected_sensor_ids) { + subscribed_topics_.insert(sensor_id.id); + } + return trajectory_id; +} + +void Node::LaunchSubscribers(const TrajectoryOptions& options, + const int trajectory_id) { + for (const std::string& topic : + ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleLaserScanMessage, trajectory_id, topic, node_, this), + topic}); + } + for (const std::string& topic : ComputeRepeatedTopicNames( + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, node_, this), + topic}); + } + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandlePointCloud2Message, trajectory_id, topic, node_, this), + topic}); + } + + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + if (node_options_.map_builder_options.use_trajectory_builder_3d() || + (node_options_.map_builder_options.use_trajectory_builder_2d() && + options.trajectory_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler(&Node::HandleImuMessage, + trajectory_id, kImuTopic, + node_, this), + kImuTopic}); + } + + if (options.use_odometry) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler(&Node::HandleOdometryMessage, + trajectory_id, kOdometryTopic, + node_, this), + kOdometryTopic}); + } + if (options.use_nav_sat) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic, + node_, this), + kNavSatFixTopic}); + } + if (options.use_landmarks) { + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, + node_, this), + kLandmarkTopic}); + } +} + +bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { + if (node_options_.map_builder_options.use_trajectory_builder_2d()) { + return options.trajectory_builder_options + .has_trajectory_builder_2d_options(); + } + if (node_options_.map_builder_options.use_trajectory_builder_3d()) { + return options.trajectory_builder_options + .has_trajectory_builder_3d_options(); + } + return false; +} + +bool Node::ValidateTopicNames(const TrajectoryOptions& options) { + for (const auto& sensor_id : ComputeExpectedSensorIds(options)) { + const std::string& topic = sensor_id.id; + if (subscribed_topics_.count(topic) > 0) { + LOG(ERROR) << "Topic name [" << topic << "] is already used."; + return false; + } + } + return true; +} + +cartographer_ros_msgs::msg::StatusResponse Node::TrajectoryStateToStatus( + const int trajectory_id, const std::set& valid_states) { + const auto trajectory_states = map_builder_bridge_->GetTrajectoryStates(); + cartographer_ros_msgs::msg::StatusResponse status_response; + + const auto it = trajectory_states.find(trajectory_id); + if (it == trajectory_states.end()) { + status_response.message = "Trajectory " + std::to_string(trajectory_id) + " doesn't exist."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND; + return status_response; + } + + status_response.message = "Trajectory " + std::to_string(trajectory_id) + " is in '" + + TrajectoryStateToString(it->second) + "' state."; + status_response.code = + valid_states.count(it->second) + ? cartographer_ros_msgs::msg::StatusCode::OK + : cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + return status_response; +} + +cartographer_ros_msgs::msg::StatusResponse Node::FinishTrajectoryUnderLock( + const int trajectory_id) { + cartographer_ros_msgs::msg::StatusResponse status_response; + if (trajectories_scheduled_for_finish_.count(trajectory_id)) { + status_response.message = + "Trajectory " + std::to_string(trajectory_id) + " already pending to finish."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::OK; + LOG(INFO) << status_response.message; + return status_response; + } + + // First, check if we can actually finish the trajectory. + status_response = TrajectoryStateToStatus( + trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); + if (status_response.code != cartographer_ros_msgs::msg::StatusCode::OK) { + LOG(ERROR) << "Can't finish trajectory: " << status_response.message; + return status_response; + } + + // Shutdown the subscribers of this trajectory. + // A valid case with no subscribers is e.g. if we just visualize states. + if (subscribers_.count(trajectory_id)) { + for (auto& entry : subscribers_[trajectory_id]) { + entry.subscriber.reset(); + subscribed_topics_.erase(entry.topic); + LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; + } + CHECK_EQ(subscribers_.erase(trajectory_id), 1); + } + map_builder_bridge_->FinishTrajectory(trajectory_id); + trajectories_scheduled_for_finish_.emplace(trajectory_id); + status_response.message = + "Finished trajectory " + std::to_string(trajectory_id) + "."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::OK; + return status_response; +} + +bool Node::handleStartTrajectory( + const cartographer_ros_msgs::srv::StartTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::StartTrajectory::Response::SharedPtr response) { + TrajectoryOptions trajectory_options; + std::tie(std::ignore, trajectory_options) = LoadOptions( + request->configuration_directory, request->configuration_basename); + + if (request->use_initial_pose) { + const auto pose = ToRigid3d(request->initial_pose); + if (!pose.IsValid()) { + response->status.message = + "Invalid pose argument. Orientation quaternion must be normalized."; + LOG(ERROR) << response->status.message; + response->status.code = + cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + return true; + } + + // Check if the requested trajectory for the relative initial pose exists. + response->status = TrajectoryStateToStatus( + request->relative_to_trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FROZEN, + TrajectoryState::FINISHED} /* valid states */); + if (response->status.code != cartographer_ros_msgs::msg::StatusCode::OK) { + LOG(ERROR) << "Can't start a trajectory with initial pose: " + << response->status.message; + return true; + } + + ::cartographer::mapping::proto::InitialTrajectoryPose + initial_trajectory_pose; + initial_trajectory_pose.set_to_trajectory_id( + request->relative_to_trajectory_id); + *initial_trajectory_pose.mutable_relative_pose() = + cartographer::transform::ToProto(pose); + initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal( + ::cartographer_ros::FromRos(rclcpp::Time(0)))); + *trajectory_options.trajectory_builder_options + .mutable_initial_trajectory_pose() = initial_trajectory_pose; + } + + if (!ValidateTrajectoryOptions(trajectory_options)) { + response->status.message = "Invalid trajectory options."; + LOG(ERROR) << response->status.message; + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + } else if (!ValidateTopicNames(trajectory_options)) { + response->status.message = "Topics are already used by another trajectory."; + LOG(ERROR) << response->status.message; + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + } else { + response->status.message = "Success."; + response->trajectory_id = AddTrajectory(trajectory_options); + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + } + return true; +} + +void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { + absl::MutexLock lock(&mutex_); + CHECK(ValidateTrajectoryOptions(options)); + AddTrajectory(options); +} + +std::vector< + std::set> +Node::ComputeDefaultSensorIdsForMultipleBags( + const std::vector& bags_options) const { + using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + std::vector> bags_sensor_ids; + for (size_t i = 0; i < bags_options.size(); ++i) { + std::string prefix; + if (bags_options.size() > 1) { + prefix = "bag_" + std::to_string(i + 1) + "_"; + } + std::set unique_sensor_ids; + for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) { + unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); + } + bags_sensor_ids.push_back(unique_sensor_ids); + } + return bags_sensor_ids; +} + +int Node::AddOfflineTrajectory( + const std::set& + expected_sensor_ids, + const TrajectoryOptions& options) { + absl::MutexLock lock(&mutex_); + const int trajectory_id = + map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); + AddExtrapolator(trajectory_id, options); + AddSensorSamplers(trajectory_id, options); + return trajectory_id; +} + +bool Node::handleGetTrajectoryStates( + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr , + cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response) { + + using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + absl::MutexLock lock(&mutex_); + response->status.code = ::cartographer_ros_msgs::msg::StatusCode::OK; + response->trajectory_states.header.stamp = node_->now(); + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { + response->trajectory_states.trajectory_id.push_back(entry.first); + switch (entry.second) { + case TrajectoryState::ACTIVE: + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::ACTIVE); + break; + case TrajectoryState::FINISHED: + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::FINISHED); + break; + case TrajectoryState::FROZEN: + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::FROZEN); + break; + case TrajectoryState::DELETED: + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::DELETED); + break; + } + } + return true; +} + +bool Node::handleFinishTrajectory( + const cartographer_ros_msgs::srv::FinishTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::FinishTrajectory::Response::SharedPtr response) { + absl::MutexLock lock(&mutex_); + response->status = FinishTrajectoryUnderLock(request->trajectory_id); + return true; +} + +bool Node::handleWriteState( + const cartographer_ros_msgs::srv::WriteState::Request::SharedPtr request, + cartographer_ros_msgs::srv::WriteState::Response::SharedPtr response) { + absl::MutexLock lock(&mutex_); + if (map_builder_bridge_->SerializeState(request->filename, + request->include_unfinished_submaps)) { + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = + "State written to '" + request->filename + "'."; + } else { + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + response->status.message = + "Failed to write '" + request->filename + "'."; + } + return true; +} + +bool Node::handleReadMetrics( + const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr, + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) { + + absl::MutexLock lock(&mutex_); + response->timestamp = node_->now(); + if (!metrics_registry_) { + response->status.code = cartographer_ros_msgs::msg::StatusCode::UNAVAILABLE; + response->status.message = "Collection of runtime metrics is not activated."; + return true; + } + metrics_registry_->ReadMetrics(response); + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = "Successfully read metrics."; + return true; +} + +void Node::FinishAllTrajectories() { + absl::MutexLock lock(&mutex_); + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { + if (entry.second == TrajectoryState::ACTIVE) { + const int trajectory_id = entry.first; + CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, + cartographer_ros_msgs::msg::StatusCode::OK); + } + } +} + +bool Node::FinishTrajectory(const int trajectory_id) { + absl::MutexLock lock(&mutex_); + return FinishTrajectoryUnderLock(trajectory_id).code == + cartographer_ros_msgs::msg::StatusCode::OK; +} + +void Node::RunFinalOptimization() { + { + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { + const int trajectory_id = entry.first; + if (entry.second == TrajectoryState::ACTIVE) { + LOG(WARNING) + << "Can't run final optimization if there are one or more active " + "trajectories. Trying to finish trajectory with ID " + << std::to_string(trajectory_id) << " now."; + CHECK(FinishTrajectory(trajectory_id)) + << "Failed to finish trajectory with ID " + << std::to_string(trajectory_id) << "."; + } + } + } + // Assuming we are not adding new data anymore, the final optimization + // can be performed without holding the mutex. + map_builder_bridge_->RunFinalOptimization(); +} + +void Node::HandleOdometryMessage(const int trajectory_id, + const std::string& sensor_id, + const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { + return; + } + auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); + auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); + if (odometry_data_ptr != nullptr) { + extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr); + } + sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); +} + +void Node::HandleNavSatFixMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { + return; + } + map_builder_bridge_->sensor_bridge(trajectory_id) + ->HandleNavSatFixMessage(sensor_id, msg); +} + +void Node::HandleLandmarkMessage( + const int trajectory_id, const std::string& sensor_id, + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { + return; + } + map_builder_bridge_->sensor_bridge(trajectory_id) + ->HandleLandmarkMessage(sensor_id, msg); +} + +void Node::HandleImuMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { + return; + } + auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); + auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); + if (imu_data_ptr != nullptr) { + extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); + } + sensor_bridge_ptr->HandleImuMessage(sensor_id, msg); +} + +void Node::HandleLaserScanMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_->sensor_bridge(trajectory_id) + ->HandleLaserScanMessage(sensor_id, msg); +} + +void Node::HandleMultiEchoLaserScanMessage( + const int trajectory_id, const std::string& sensor_id, + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_->sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage(sensor_id, msg); +} + +void Node::HandlePointCloud2Message( + const int trajectory_id, const std::string& sensor_id, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { + absl::MutexLock lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } + map_builder_bridge_->sensor_bridge(trajectory_id) + ->HandlePointCloud2Message(sensor_id, msg); +} + +void Node::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { + absl::MutexLock lock(&mutex_); + CHECK( + map_builder_bridge_->SerializeState(filename, include_unfinished_submaps)) + << "Could not write state."; +} + +void Node::LoadState(const std::string& state_filename, + const bool load_frozen_state) { + absl::MutexLock lock(&mutex_); + map_builder_bridge_->LoadState(state_filename, load_frozen_state); +} + +// TODO: find ROS equivalent to ros::master::getTopics +void Node::MaybeWarnAboutTopicMismatch() { +// ::ros::master::V_TopicInfo ros_topics; +// ::ros::master::getTopics(ros_topics); +// std::set published_topics; +// std::stringstream published_topics_string; +// for (const auto& it : ros_topics) { +// std::string resolved_topic = node_handle_.resolveName(it.name, false); +// published_topics.insert(resolved_topic); +// published_topics_string << resolved_topic << ","; +// } +// bool print_topics = false; +// for (const auto& entry : subscribers_) { +// int trajectory_id = entry.first; +// for (const auto& subscriber : entry.second) { +// std::string resolved_topic = node_handle_.resolveName(subscriber.topic); +// if (published_topics.count(resolved_topic) == 0) { +// LOG(WARNING) << "Expected topic \"" << subscriber.topic +// << "\" (trajectory " << trajectory_id << ")" +// << " (resolved topic \"" << resolved_topic << "\")" +// << " but no publisher is currently active."; +// print_topics = true; +// } +// } +// } +// if (print_topics) { +// LOG(WARNING) << "Currently available topics are: " +// << published_topics_string.str(); +// } +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_constants.cc b/cartographer_ros/src/node_constants.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/node_constants.cc rename to cartographer_ros/src/node_constants.cpp diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/src/node_main.cpp similarity index 72% rename from cartographer_ros/cartographer_ros/node_main.cc rename to cartographer_ros/src/node_main.cpp index e38a6edfc..f403be079 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/src/node_main.cpp @@ -14,14 +14,17 @@ * limitations under the License. */ +#include "absl/memory/memory.h" #include "cartographer/mapping/map_builder.h" #include "cartographer_ros/node.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/ros_log_sink.h" #include "gflags/gflags.h" +#include "tf2_ros/transform_listener.h" -#include - +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " "second is always the Cartographer installation to allow " @@ -41,22 +44,32 @@ DEFINE_string( save_state_filename, "", "If non-empty, serialize state and write it to disk before shutting down."); - namespace cartographer_ros { namespace { void Run() { + rclcpp::Node::SharedPtr cartographer_node = rclcpp::Node::make_shared("cartographer_node"); + constexpr double kTfBufferCacheTimeInSeconds = 10.; + + std::shared_ptr tf_buffer = + std::make_shared( + cartographer_node->get_clock(), + tf2::durationFromSec(kTfBufferCacheTimeInSeconds), + cartographer_node); + + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + NodeOptions node_options; TrajectoryOptions trajectory_options; std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); auto map_builder = - cartographer::common::make_unique( - node_options.map_builder_options); - - auto node = std::make_shared(node_options, std::move(map_builder)); - + cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); + auto node = std::make_shared( + node_options, std::move(map_builder), tf_buffer, cartographer_node, + FLAGS_collect_metrics); if (!FLAGS_load_state_filename.empty()) { node->LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } @@ -65,13 +78,14 @@ void Run() { node->StartTrajectoryWithDefaultTopics(trajectory_options); } - rclcpp::spin(node); + rclcpp::spin(cartographer_node); node->FinishAllTrajectories(); node->RunFinalOptimization(); if (!FLAGS_save_state_filename.empty()) { - node->SerializeState(FLAGS_save_state_filename); + node->SerializeState(FLAGS_save_state_filename, + true /* include_unfinished_submaps */); } } @@ -80,10 +94,8 @@ void Run() { int main(int argc, char** argv) { // Init rclcpp first because gflags reorders command line flags in argv - ::rclcpp::init(argc, argv); + rclcpp::init(argc, argv); - // Keep going if an unknown flag is encountered - // https://github.com/gflags/gflags/issues/148#issuecomment-318826625 google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); google::ParseCommandLineFlags(&argc, &argv, false); diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/src/node_options.cpp similarity index 75% rename from cartographer_ros/cartographer_ros/node_options.cc rename to cartographer_ros/src/node_options.cpp index 004006c6e..681754e7e 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/src/node_options.cpp @@ -19,7 +19,7 @@ #include #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" #include "glog/logging.h" namespace cartographer_ros { @@ -40,15 +40,27 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); options.trajectory_publish_period_sec = lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec"); + if (lua_parameter_dictionary->HasKey("publish_to_tf")) { + options.publish_to_tf = + lua_parameter_dictionary->GetBool("publish_to_tf"); + } + if (lua_parameter_dictionary->HasKey("publish_tracked_pose")) { + options.publish_tracked_pose = + lua_parameter_dictionary->GetBool("publish_tracked_pose"); + } + if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) { + options.use_pose_extrapolator = + lua_parameter_dictionary->GetBool("use_pose_extrapolator"); + } return options; } std::tuple LoadOptions( const std::string& configuration_directory, const std::string& configuration_basename) { - auto file_resolver = cartographer::common::make_unique< - cartographer::common::ConfigurationFileResolver>( - std::vector{configuration_directory}); + auto file_resolver = + absl::make_unique( + std::vector{configuration_directory}); const std::string code = file_resolver->GetFileContentOrDie(configuration_basename); cartographer::common::LuaParameterDictionary lua_parameter_dictionary( diff --git a/cartographer_ros/src/occupancy_grid_node_main.cpp b/cartographer_ros/src/occupancy_grid_node_main.cpp new file mode 100644 index 000000000..324426bf9 --- /dev/null +++ b/cartographer_ros/src/occupancy_grid_node_main.cpp @@ -0,0 +1,208 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * 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. + */ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" +#include "cairo/cairo.h" +#include "cartographer/common/port.h" +#include "cartographer/io/image.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/submap.h" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include "gflags/gflags.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include + +DEFINE_double(resolution, 0.05, + "Resolution of a grid cell in the published occupancy grid."); +DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period."); +DEFINE_bool(include_frozen_submaps, true, + "Include frozen submaps in the occupancy grid."); +DEFINE_bool(include_unfrozen_submaps, true, + "Include unfrozen submaps in the occupancy grid."); +DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic, + "Name of the topic on which the occupancy grid is published."); + +namespace cartographer_ros { +namespace { + +using ::cartographer::io::PaintSubmapSlicesResult; +using ::cartographer::io::SubmapSlice; +using ::cartographer::mapping::SubmapId; + +class Node : public rclcpp::Node +{ + public: + explicit Node(double resolution, double publish_period_sec); + ~Node() {} + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + + private: + void HandleSubmapList(const cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr& msg); + void DrawAndPublish(); + + const double resolution_; + + absl::Mutex mutex_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; + ::rclcpp::Client::SharedPtr client_ GUARDED_BY(mutex_); + ::rclcpp::Subscription::SharedPtr submap_list_subscriber_ GUARDED_BY(mutex_); + ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ GUARDED_BY(mutex_); + std::map submap_slices_ GUARDED_BY(mutex_); + rclcpp::TimerBase::SharedPtr occupancy_grid_publisher_timer_; + std::string last_frame_id_; + rclcpp::Time last_timestamp_; +}; + +Node::Node(const double resolution, const double publish_period_sec) + : rclcpp::Node("cartographer_occupancy_grid_node"), + resolution_(resolution) +{ + callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface()); + client_ = this->create_client( + kSubmapQueryServiceName, + rmw_qos_profile_services_default, + callback_group_ + ); + + occupancy_grid_publisher_ = this->create_publisher<::nav_msgs::msg::OccupancyGrid>( + kOccupancyGridTopic, rclcpp::QoS(10).transient_local()); + + occupancy_grid_publisher_timer_ = this->create_wall_timer( + std::chrono::milliseconds(int(publish_period_sec * 1000)), + [this]() { + DrawAndPublish(); + }); + + auto handleSubmapList = + [this, publish_period_sec](const typename cartographer_ros_msgs::msg::SubmapList::SharedPtr msg) -> void + { + absl::MutexLock locker(&mutex_); + + // We do not do any work if nobody listens. + if (this->count_publishers(kSubmapListTopic) == 0){ + return; + } + + // Keep track of submap IDs that don't appear in the message anymore. + std::set submap_ids_to_delete; + for (const auto& pair : submap_slices_) { + submap_ids_to_delete.insert(pair.first); + } + + for (const auto& submap_msg : msg->submap) { + const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; + submap_ids_to_delete.erase(id); + if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) || + (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) { + continue; + } + SubmapSlice& submap_slice = submap_slices_[id]; + submap_slice.pose = ToRigid3d(submap_msg.pose); + submap_slice.metadata_version = submap_msg.submap_version; + if (submap_slice.surface != nullptr && + submap_slice.version == submap_msg.submap_version) { + continue; + } + + auto fetched_textures = cartographer_ros::FetchSubmapTextures( + id, client_, callback_group_executor_, + std::chrono::milliseconds(int(publish_period_sec * 1000))); + if (fetched_textures == nullptr) { + continue; + } + CHECK(!fetched_textures->textures.empty()); + submap_slice.version = fetched_textures->version; + + // We use the first texture only. By convention this is the highest + // resolution texture and that is the one we want to use to construct the + // map for ROS. + const auto fetched_texture = fetched_textures->textures.begin(); + submap_slice.width = fetched_texture->width; + submap_slice.height = fetched_texture->height; + submap_slice.slice_pose = fetched_texture->slice_pose; + submap_slice.resolution = fetched_texture->resolution; + submap_slice.cairo_data.clear(); + submap_slice.surface = ::cartographer::io::DrawTexture( + fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, + fetched_texture->width, fetched_texture->height, + &submap_slice.cairo_data); + } + + // Delete all submaps that didn't appear in the message. + for (const auto& id : submap_ids_to_delete) { + submap_slices_.erase(id); + } + + last_timestamp_ = msg->header.stamp; + last_frame_id_ = msg->header.frame_id; + }; + + submap_list_subscriber_ = create_subscription( + kSubmapListTopic, rclcpp::QoS(10), handleSubmapList); +} + +void Node::DrawAndPublish() { + absl::MutexLock locker(&mutex_); + if (submap_slices_.empty() || last_frame_id_.empty()) { + return; + } + auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); + std::unique_ptr msg_ptr = CreateOccupancyGridMsg( + painted_slices, resolution_, last_frame_id_, last_timestamp_); + occupancy_grid_publisher_->publish(*msg_ptr); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, false); + + CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps) + << "Ignoring both frozen and unfrozen submaps makes no sense."; + + cartographer_ros::ScopedRosLogSink ros_log_sink; + auto node = std::make_shared(FLAGS_resolution, FLAGS_publish_period_sec); + + rclcpp::spin(node); + ::rclcpp::shutdown(); + return 0; +} diff --git a/cartographer_ros/src/offline_node.cpp b/cartographer_ros/src/offline_node.cpp new file mode 100644 index 000000000..3cfa9dcd5 --- /dev/null +++ b/cartographer_ros/src/offline_node.cpp @@ -0,0 +1,462 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/offline_node.h" + +#include +#include +#ifndef WIN32 +#include +#endif +#include + +#include + +#include "cartographer_ros/node.h" +#include "cartographer_ros/playable_bag.h" +#include "cartographer_ros/urdf_reader.h" +#include "gflags/gflags.h" +#include "rosgraph_msgs/msg/clock.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "urdf/model.h" +#include "rclcpp/exceptions.hpp" +#include +#include + +DEFINE_bool(collect_metrics, false, + "Activates the collection of runtime metrics. If activated, the " + "metrics can be accessed via a ROS service."); +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string( + configuration_basenames, "", + "Comma-separated list of basenames, i.e. not containing any " + "directory prefix, of the configuration files for each trajectory. " + "The first configuration file will be used for node options. " + "If less configuration files are specified than trajectories, the " + "first file will be used the remaining trajectories."); +DEFINE_string( + bag_filenames, "", + "Comma-separated list of bags to process. One bag per trajectory. " + "Any combination of simultaneous and sequential bags is supported."); +DEFINE_string(urdf_filenames, "", + "Comma-separated list of one or more URDF files that contain " + "static links for the sensor configuration(s)."); +DEFINE_bool(use_bag_transforms, true, + "Whether to read, use and republish transforms from bags."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file to load, containing " + "a saved SLAM state."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_string(save_state_filename, "", + "Explicit name of the file to which the serialized state will be " + "written before shutdown. If left empty, the filename will be " + "inferred from the first bagfile's name as: " + ".pbstream"); +DEFINE_bool(keep_running, false, + "Keep running the offline node after all messages from the bag " + "have been processed."); +DEFINE_double(skip_seconds, 0, + "Optional amount of seconds to skip from the beginning " + "(i.e. when the earliest bag starts.). "); + +namespace cartographer_ros { + +constexpr char kClockTopic[] = "clock"; +constexpr char kTfStaticTopic[] = "/tf_static"; +constexpr char kTfTopic[] = "/tf"; +constexpr double kClockPublishFrequencySec = 1. / 30.; +constexpr int kSingleThreaded = 1; +// We publish tf messages one second earlier than other messages. Under +// the assumption of higher frequency tf this should ensure that tf can +// always interpolate. +const rclcpp::Duration kDelay(1.0, 0); + +void RunOfflineNode(const MapBuilderFactory& map_builder_factory, + rclcpp::Node::SharedPtr cartographer_offline_node) { + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + LOG(WARNING) << "FLAGS_configuration_directory " << FLAGS_configuration_directory; + CHECK(!FLAGS_configuration_basenames.empty()) + << "-configuration_basenames is missing."; + LOG(WARNING) << "FLAGS_configuration_basenames " << FLAGS_configuration_basenames; + CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) + << "-bag_filenames and -load_state_filename cannot both be unspecified."; + std::regex regex(","); + std::vector bag_filenames; + if (!FLAGS_bag_filenames.empty()){ + std::regex regex(","); + std::vector if_bag_filenames( + std::sregex_token_iterator( + FLAGS_bag_filenames.begin(), FLAGS_bag_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); + bag_filenames = if_bag_filenames; + } + cartographer_ros::NodeOptions node_options; + std::vector configuration_basenames( + std::sregex_token_iterator( + FLAGS_configuration_basenames.begin(), FLAGS_configuration_basenames.end(), regex, -1), + std::sregex_token_iterator() + ); + + std::vector bag_trajectory_options(1); + std::tie(node_options, bag_trajectory_options.at(0)) = + LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0)); + + for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) { + TrajectoryOptions current_trajectory_options; + if (bag_index < configuration_basenames.size()) { + std::tie(std::ignore, current_trajectory_options) = LoadOptions( + FLAGS_configuration_directory, configuration_basenames.at(bag_index)); + } else { + current_trajectory_options = bag_trajectory_options.at(0); + } + bag_trajectory_options.push_back(current_trajectory_options); + } + if (bag_filenames.size() > 0) { + CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size()); + } + + // Since we preload the transform buffer, we should never have to wait for a + // transform. When we finish processing the bag, we will simply drop any + // remaining sensor data that cannot be transformed due to missing transforms. + node_options.lookup_transform_timeout_sec = 0.; + + auto map_builder = map_builder_factory(node_options.map_builder_options); + + const std::chrono::time_point start_time = + std::chrono::steady_clock::now(); + + std::shared_ptr tf_buffer = + std::make_shared( + cartographer_offline_node->get_clock(), + tf2::durationFromSec(10), + cartographer_offline_node); + + std::vector urdf_transforms; + + if (!FLAGS_urdf_filenames.empty()) { + std::vector urdf_filenames( + std::sregex_token_iterator( + FLAGS_urdf_filenames.begin(), FLAGS_urdf_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); + for (const auto& urdf_filename : urdf_filenames) { + const auto current_urdf_transforms = + ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer); + urdf_transforms.insert(urdf_transforms.end(), + current_urdf_transforms.begin(), + current_urdf_transforms.end()); + } + } + tf_buffer->setUsingDedicatedThread(true); + + Node node(node_options, std::move(map_builder), tf_buffer, cartographer_offline_node, + FLAGS_collect_metrics); + if (!FLAGS_load_state_filename.empty()) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); + } + + rclcpp::Publisher::SharedPtr tf_publisher = + cartographer_offline_node->create_publisher( + kTfTopic, kLatestOnlyPublisherQueueSize); + + ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster(cartographer_offline_node); + + rclcpp::Publisher::SharedPtr clock_publisher = + cartographer_offline_node->create_publisher( + kClockTopic, kLatestOnlyPublisherQueueSize); + + if (urdf_transforms.size() > 0) { + static_tf_broadcaster.sendTransform(urdf_transforms); + } + + rosgraph_msgs::msg::Clock clock; + + std::vector< + std::set> + bag_expected_sensor_ids; + if (configuration_basenames.size() == 1) { + const auto current_bag_expected_sensor_ids = + node.ComputeDefaultSensorIdsForMultipleBags( + {bag_trajectory_options.front()}); + bag_expected_sensor_ids = {bag_filenames.size(), + current_bag_expected_sensor_ids.front()}; + } else { + bag_expected_sensor_ids = + node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options); + } + CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size()); + + std::map, + cartographer::mapping::TrajectoryBuilderInterface::SensorId> + bag_topic_to_sensor_id; + PlayableBagMultiplexer playable_bag_multiplexer(cartographer_offline_node); + for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size(); + ++current_bag_index) { + const std::string& bag_filename = bag_filenames.at(current_bag_index); + if (!rclcpp::ok()) { + return; + } + for (const auto& expected_sensor_id : + bag_expected_sensor_ids.at(current_bag_index)) { + LOG(INFO) << "expected_sensor_id.id " << expected_sensor_id.id; + const auto bag_resolved_topic = std::make_pair( + static_cast(current_bag_index), + "/"+expected_sensor_id.id); + if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) { + LOG(ERROR) << "Sensor /" << expected_sensor_id.id << " of bag " + << current_bag_index << " resolves to topic " + << bag_resolved_topic.second << " which is already used by " + << " sensor " + << bag_topic_to_sensor_id.at(bag_resolved_topic).id; + } + bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id; + } + + auto serializer = rclcpp::Serialization(); + playable_bag_multiplexer.AddPlayableBag(PlayableBag( + bag_filename, current_bag_index, kDelay, + // PlayableBag::FilteringEarlyMessageHandler is used to get an early + // peek at the tf messages in the bag and insert them into 'tf_buffer'. + // When a message is retrieved by GetNextMessage() further below, + // we will have already inserted further 'kDelay' seconds worth of + // transforms into 'tf_buffer' via this lambda. + [&tf_publisher, tf_buffer, cartographer_offline_node, serializer](std::shared_ptr msg) { + // TODO: filter bag msg per type ? Planned rosbag2 evolution ? + if (msg->topic_name == kTfTopic || msg->topic_name == kTfStaticTopic) { + if (FLAGS_use_bag_transforms) { + tf2_msgs::msg::TFMessage tf_message; + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + try { + serializer.deserialize_message(&serialized_msg, &tf_message); + for (auto& transform : tf_message.transforms) { + try { + // We need to keep 'tf_buffer' small because it becomes very + // inefficient otherwise. We make sure that tf_messages are + // published before any data messages, so that tf lookups + // always work. + tf_buffer->setTransform(transform, "unused_authority", + msg->topic_name == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + } + tf_publisher->publish(tf_message); + } catch (const rclcpp::exceptions::RCLError& rcl_error) { + return true; + } + } + // Tell 'PlayableBag' to filter the tf message since there is no + // further use for it. + return false; + } else { + return true; + } + })); + } + + std::set bag_topics; + std::stringstream bag_topics_string; + for (const auto& topic : playable_bag_multiplexer.topics()) { + std::string resolved_topic = cartographer_offline_node->get_node_base_interface()-> + resolve_topic_or_service_name(topic, false); + bag_topics.insert(resolved_topic); + bag_topics_string << resolved_topic << ","; + } + bool print_topics = false; + for (const auto& entry : bag_topic_to_sensor_id) { + const std::string& resolved_topic = entry.first.second; + if (bag_topics.count(resolved_topic) == 0) { + LOG(WARNING) << "Expected resolved topic \"" << resolved_topic + << "\" not found in bag file(s)."; + print_topics = true; + } + } + if (print_topics) { + LOG(WARNING) << "Available topics in bag file(s) are " + << bag_topics_string.str(); + } + + std::unordered_map bag_index_to_trajectory_id; + const rclcpp::Time begin_time = + // If no bags were loaded, we cannot peek the time of first message. + playable_bag_multiplexer.IsMessageAvailable() + ? playable_bag_multiplexer.PeekMessageTime() + : rclcpp::Time(); + + auto laser_scan_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto pcl2_serializer = rclcpp::Serialization(); + auto imu_serializer = rclcpp::Serialization(); + auto odom_serializer = rclcpp::Serialization(); + auto nav_sat_fix_serializer = rclcpp::Serialization(); + auto landmark_list_serializer = rclcpp::Serialization(); + + while (playable_bag_multiplexer.IsMessageAvailable()) { + if (!::rclcpp::ok()) { + return; + } + + const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); + const rosbag2_storage::SerializedBagMessage& msg = std::get<0>(next_msg_tuple); + const int bag_index = std::get<1>(next_msg_tuple); + const std::string topic_type = std::get<2>(next_msg_tuple); + const bool is_last_message_in_bag = std::get<3>(next_msg_tuple); + + if (msg.time_stamp < + (begin_time.nanoseconds() + rclcpp::Duration(FLAGS_skip_seconds, 0).nanoseconds())) { + continue; + } + + int trajectory_id; + // Lazily add trajectories only when the first message arrives in order + // to avoid blocking the sensor queue. + if (bag_index_to_trajectory_id.count(bag_index) == 0) { + trajectory_id = + node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index), + bag_trajectory_options.at(bag_index)); + CHECK(bag_index_to_trajectory_id + .emplace(std::piecewise_construct, + std::forward_as_tuple(bag_index), + std::forward_as_tuple(trajectory_id)) + .second); + LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag " + << bag_filenames.at(bag_index); + } else { + trajectory_id = bag_index_to_trajectory_id.at(bag_index); + } + + const auto bag_topic = std::make_pair( + bag_index, + cartographer_offline_node->get_node_base_interface()-> + resolve_topic_or_service_name(msg.topic_name, false)); + auto it = bag_topic_to_sensor_id.find(bag_topic); + + if (it != bag_topic_to_sensor_id.end()) { + const std::string& sensor_id = it->second.id; + + if (topic_type == "sensor_msgs/msg/LaserScan") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::LaserScan::SharedPtr laser_scan_msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, laser_scan_msg.get()); + node.HandleLaserScanMessage(trajectory_id, sensor_id, + laser_scan_msg); + } else if (topic_type == "sensor_msgs/msg/MultiEchoLaserScan") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, multi_echo_laser_scan_msg.get()); + node.HandleMultiEchoLaserScanMessage(trajectory_id, sensor_id, + multi_echo_laser_scan_msg); + } else if (topic_type == "sensor_msgs/msg/PointCloud2") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, pcl2_scan_msg.get()); + node.HandlePointCloud2Message(trajectory_id, sensor_id, + pcl2_scan_msg); + } else if (topic_type == "sensor_msgs/msg/Imu") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::Imu::SharedPtr imu_scan_msg = + std::make_shared(); + imu_serializer.deserialize_message(&serialized_msg, imu_scan_msg.get()); + node.HandleImuMessage(trajectory_id, sensor_id, + imu_scan_msg); + } else if (topic_type == "nav_msgs/msg/Odometry") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + nav_msgs::msg::Odometry::SharedPtr odom_scan_msg = + std::make_shared(); + odom_serializer.deserialize_message(&serialized_msg, odom_scan_msg.get()); + node.HandleOdometryMessage(trajectory_id, sensor_id, + odom_scan_msg); + } else if (topic_type == "sensor_msgs/msg/NavSatFix") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::NavSatFix::SharedPtr nav_sat_fix_msg = + std::make_shared(); + nav_sat_fix_serializer.deserialize_message(&serialized_msg, nav_sat_fix_msg.get()); + node.HandleNavSatFixMessage(trajectory_id, sensor_id, + nav_sat_fix_msg); + } else if (topic_type == "cartographer_ros_msgs/msg/LandmarkList") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + cartographer_ros_msgs::msg::LandmarkList::SharedPtr landmark_list_msg = + std::make_shared(); + landmark_list_serializer.deserialize_message(&serialized_msg, landmark_list_msg.get()); + node.HandleLandmarkMessage(trajectory_id, sensor_id, + landmark_list_msg); + } + } + clock.clock = rclcpp::Time(msg.time_stamp); + clock_publisher->publish(clock); + rclcpp::spin_some(cartographer_offline_node); + + if (is_last_message_in_bag) { + node.FinishTrajectory(trajectory_id); + } + } + + // Ensure the clock is republished after the bag has been finished, during the + // final optimization, serialization, and optional indefinite spinning at the + // end. + // TODO: need a spin for the timer to tick + auto clock_republish_timer = cartographer_offline_node->create_wall_timer( + std::chrono::milliseconds(int(kClockPublishFrequencySec)), + [&clock_publisher, &clock]() { + clock_publisher->publish(clock); + }); + node.RunFinalOptimization(); + + const std::chrono::time_point end_time = + std::chrono::steady_clock::now(); + const double wall_clock_seconds = + std::chrono::duration_cast>(end_time - + start_time) + .count(); + + LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s"; +#ifdef __linux__ + timespec cpu_timespec = {}; + clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec); + LOG(INFO) << "Elapsed CPU time: " + << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s"; + rusage usage; + CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno); + LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB"; +#endif + + // Serialize unless we have neither a bagfile nor an explicit state filename. + if (rclcpp::ok() && + !(bag_filenames.empty() && FLAGS_save_state_filename.empty())) { + const std::string state_output_filename = + FLAGS_save_state_filename.empty() + ? bag_filenames.front() + ".pbstream" + : FLAGS_save_state_filename; + LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; + node.SerializeState(state_output_filename, + true /* include_unfinished_submaps */); + } + if (FLAGS_keep_running) { + LOG(INFO) << "Finished processing and waiting for shutdown."; + rclcpp::spin(cartographer_offline_node); + } +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/src/offline_node_main.cpp similarity index 63% rename from cartographer_ros/cartographer_ros/offline_node_main.cc rename to cartographer_ros/src/offline_node_main.cpp index 845954f26..b1c2bf64e 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/src/offline_node_main.cpp @@ -18,25 +18,26 @@ #include "cartographer_ros/offline_node.h" #include "cartographer_ros/ros_log_sink.h" #include "gflags/gflags.h" -#include "ros/ros.h" +#include int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + rclcpp::init(argc, argv); - ::ros::init(argc, argv, "cartographer_offline_node"); - ::ros::start(); + google::AllowCommandLineReparsing(); + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, false); cartographer_ros::ScopedRosLogSink ros_log_sink; - const cartographer_ros::MapBuilderFactory map_builder_factory = - [](const ::cartographer::mapping::proto::MapBuilderOptions& - map_builder_options) { - return ::cartographer::common::make_unique< - ::cartographer::mapping::MapBuilder>(map_builder_options); - }; + const cartographer_ros::MapBuilderFactory map_builder_factory = []( + const ::cartographer::mapping::proto::MapBuilderOptions& + map_builder_options) { + return ::cartographer::mapping::CreateMapBuilder(map_builder_options); + }; - cartographer_ros::RunOfflineNode(map_builder_factory); + rclcpp::Node::SharedPtr cartographer_offline_node = + rclcpp::Node::make_shared("cartographer_offline_node"); + cartographer_ros::RunOfflineNode(map_builder_factory, cartographer_offline_node); - ::ros::shutdown(); + rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/src/pbstream_map_publisher_main.cpp similarity index 63% rename from cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc rename to cartographer_ros/src/pbstream_map_publisher_main.cpp index 4774ac8ef..8d149be65 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/src/pbstream_map_publisher_main.cpp @@ -25,19 +25,14 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/3d/submap_3d.h" -#include "cartographer/mapping/proto/pose_graph.pb.h" -#include "cartographer/mapping/proto/serialization.pb.h" -#include "cartographer/mapping/proto/submap.pb.h" -#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/node_constants.h" #include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_map.h" -#include "cartographer_ros/submap.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "nav_msgs/OccupancyGrid.h" -#include "ros/ros.h" +#include +#include DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from."); @@ -48,64 +43,59 @@ DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); namespace cartographer_ros { namespace { -void Run(const std::string& pbstream_filename, const std::string& map_topic, - const std::string& map_frame_id, const double resolution) { +std::unique_ptr LoadOccupancyGridMsg( + const std::string& pbstream_filename, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); LOG(INFO) << "Loading submap slices from serialized data."; std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices; - ::cartographer::mapping::proto::SerializedData proto; - while (deserializer.ReadNextSerializedData(&proto)) { - if (proto.has_submap()) { - const auto& submap = proto.submap(); - const ::cartographer::mapping::SubmapId id{ - submap.submap_id().trajectory_id(), - submap.submap_id().submap_index()}; - const ::cartographer::transform::Rigid3d global_submap_pose = - ::cartographer::transform::ToRigid3(deserializer.pose_graph() - .trajectory(id.trajectory_id) - .submap(id.submap_index) - .pose()); - FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); - } - } + ::cartographer::mapping::ValueConversionTables conversion_tables; + ::cartographer::io::DeserializeAndFillSubmapSlices( + &deserializer, &submap_slices, &conversion_tables); CHECK(reader.eof()); - ::ros::NodeHandle node_handle(""); - ::ros::Publisher pub = node_handle.advertise( - map_topic, kLatestOnlyPublisherQueueSize, true /*latched */); - LOG(INFO) << "Generating combined map image from submap slices."; const auto painted_slices = ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); - std::unique_ptr msg_ptr = CreateOccupancyGridMsg( - painted_slices, resolution, FLAGS_map_frame_id, ros::Time::now()); + return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id, + rclcpp::Clock().now()); +} + +void Run(const std::string& pbstream_filename, const std::string& map_topic, + const std::string& map_frame_id, const double resolution) { + rclcpp::Node::SharedPtr cartographer_pbstream_map_publisher_node = + rclcpp::Node::make_shared("cartographer_pbstream_map_publisher"); + std::unique_ptr msg_ptr = + LoadOccupancyGridMsg(pbstream_filename, resolution); + + auto pub = cartographer_pbstream_map_publisher_node->create_publisher( + map_topic, rclcpp::QoS(1).transient_local()); LOG(INFO) << "Publishing occupancy grid topic " << map_topic << " (frame_id: " << map_frame_id << ", resolution:" << std::to_string(resolution) << ")."; - pub.publish(*msg_ptr); - ::ros::spin(); - ::ros::shutdown(); + pub->publish(*msg_ptr); + rclcpp::spin(cartographer_pbstream_map_publisher_node); } } // namespace } // namespace cartographer_ros int main(int argc, char** argv) { - FLAGS_alsologtostderr = true; + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; - ::ros::init(argc, argv, "cartographer_pbstream_map_publisher"); - ::ros::start(); - cartographer_ros::ScopedRosLogSink ros_log_sink; ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic, FLAGS_map_frame_id, FLAGS_resolution); + ::rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/src/pbstream_to_ros_map_main.cpp similarity index 73% rename from cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc rename to cartographer_ros/src/pbstream_to_ros_map_main.cpp index b75fcc0e5..1ef4539a7 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/src/pbstream_to_ros_map_main.cpp @@ -23,14 +23,10 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/3d/submap_3d.h" -#include "cartographer/mapping/proto/pose_graph.pb.h" -#include "cartographer/mapping/proto/serialization.pb.h" -#include "cartographer/mapping/proto/submap.pb.h" -#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer_ros/ros_map.h" -#include "cartographer_ros/submap.h" #include "gflags/gflags.h" #include "glog/logging.h" +#include DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from."); @@ -45,26 +41,12 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem, ::cartographer::io::ProtoStreamReader reader(pbstream_filename); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); - const auto& pose_graph = deserializer.pose_graph(); - LOG(INFO) << "Loading submap slices from serialized data."; std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices; - ::cartographer::mapping::proto::SerializedData proto; - while (deserializer.ReadNextSerializedData(&proto)) { - if (proto.has_submap()) { - const auto& submap = proto.submap(); - const ::cartographer::mapping::SubmapId id{ - submap.submap_id().trajectory_id(), - submap.submap_id().submap_index()}; - const ::cartographer::transform::Rigid3d global_submap_pose = - ::cartographer::transform::ToRigid3( - pose_graph.trajectory(id.trajectory_id) - .submap(id.submap_index) - .pose()); - FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); - } - } + ::cartographer::mapping::ValueConversionTables conversion_tables; + ::cartographer::io::DeserializeAndFillSubmapSlices( + &deserializer, &submap_slices, &conversion_tables); CHECK(reader.eof()); LOG(INFO) << "Generating combined map image from submap slices."; @@ -88,13 +70,18 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem, } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing."; ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem, FLAGS_resolution); + rclcpp::shutdown(); } diff --git a/cartographer_ros/src/playable_bag.cpp b/cartographer_ros/src/playable_bag.cpp new file mode 100644 index 000000000..f9ef8fcd4 --- /dev/null +++ b/cartographer_ros/src/playable_bag.cpp @@ -0,0 +1,191 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/playable_bag.h" + +#include "absl/memory/memory.h" +#include "cartographer_ros/node_constants.h" +#include "glog/logging.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "rosbag2_storage/topic_metadata.hpp" +#include "rosbag2_storage/bag_metadata.hpp" + +namespace cartographer_ros { + +PlayableBag::PlayableBag( + const std::string& bag_filename, const int bag_id, + const rclcpp::Duration buffer_delay, + FilteringEarlyMessageHandler filtering_early_message_handler) + : bag_reader_(std::make_unique()), + finished_(false), + bag_id_(bag_id), + bag_filename_(bag_filename), + message_counter_(0), + buffer_delay_(buffer_delay), + filtering_early_message_handler_( + std::move(filtering_early_message_handler)) { + LOG(WARNING) << "Opening bag: " << bag_filename; + bag_reader_->open(bag_filename); + bag_metadata = bag_reader_->get_metadata(); + duration_in_seconds_ = bag_metadata.duration.count()/1e9; + LOG(WARNING) << "duration_in_seconds_: " << duration_in_seconds_; + LOG(WARNING) << "message_count: " << bag_metadata.message_count; + LOG(WARNING) << "compression_mode: " << bag_metadata.compression_mode; + LOG(WARNING) << "compression_format: " << bag_metadata.compression_format; + AdvanceUntilMessageAvailable(); + for (auto topic_info : bag_metadata.topics_with_message_count) { + topics_.insert(topic_info.topic_metadata.name); + } +} + +rclcpp::Time PlayableBag::PeekMessageTime() const { + CHECK(IsMessageAvailable()); + return rclcpp::Time(buffered_messages_.front().time_stamp); +} + +std::tuple PlayableBag::GetBeginEndTime() const { + return std::make_tuple( + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count()), + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count() + bag_metadata.duration.count())); +} + +rosbag2_storage::SerializedBagMessage PlayableBag::GetNextMessage( + cartographer_ros_msgs::msg::BagfileProgress* progress) { + CHECK(IsMessageAvailable()); + const rosbag2_storage::SerializedBagMessage msg = buffered_messages_.front(); + buffered_messages_.pop_front(); + AdvanceUntilMessageAvailable(); + double processed_seconds = + (rclcpp::Time(msg.time_stamp) - + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count())).seconds(); + if ((message_counter_ % 10000) == 0) { + LOG(INFO) << "Processed " << processed_seconds << " of " + << duration_in_seconds_ << " seconds of bag " << bag_filename_; + } + + if (progress) { + progress->current_bagfile_name = bag_filename_; + progress->current_bagfile_id = bag_id_; + progress->total_messages = bag_metadata.message_count; + progress->processed_messages = message_counter_; + progress->total_seconds = duration_in_seconds_; + progress->processed_seconds = processed_seconds; + } + + return msg; +} + +bool PlayableBag::IsMessageAvailable() const { + return !buffered_messages_.empty() && + (buffered_messages_.front().time_stamp < + buffered_messages_.back().time_stamp - buffer_delay_.nanoseconds()); +} + +int PlayableBag::bag_id() const { return bag_id_; } + +void PlayableBag::AdvanceOneMessage() { + CHECK(!finished_); + if (!bag_reader_->has_next()) { + finished_ = true; + return; + } + auto msg = bag_reader_->read_next(); + if (!filtering_early_message_handler_ || + filtering_early_message_handler_(msg)) { + buffered_messages_.push_back(*msg); + } + ++message_counter_; +} + +void PlayableBag::AdvanceUntilMessageAvailable() { + if (finished_) { + return; + } + do { + AdvanceOneMessage(); + } while (!finished_ && !IsMessageAvailable()); +} + +PlayableBagMultiplexer::PlayableBagMultiplexer(rclcpp::Node::SharedPtr node) { + node_ = node; + bag_progress_pub_ = node_->create_publisher( + "bagfile_progress", 10); + if (!node_->has_parameter("bagfile_progress_pub_interval")) { + node_->declare_parameter("bagfile_progress_pub_interval", progress_pub_interval_); + } + progress_pub_interval_ = + node_->get_parameter_or("bagfile_progress_pub_interval", progress_pub_interval_, 10.0); +} + +void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { + for (const auto& topic : playable_bag.topics()) { + topics_.insert(topic); + } + playable_bags_.push_back(std::move(playable_bag)); + CHECK(playable_bags_.back().IsMessageAvailable()); + next_message_queue_.emplace( + BagMessageItem{playable_bags_.back().PeekMessageTime(), + static_cast(playable_bags_.size() - 1)}); + bag_progress_time_map_[playable_bag.bag_id()] = node_->now(); +} + +bool PlayableBagMultiplexer::IsMessageAvailable() const { + return !next_message_queue_.empty(); +} + +std::tuple PlayableBagMultiplexer::GetNextMessage() { + CHECK(IsMessageAvailable()); + const int current_bag_index = next_message_queue_.top().bag_index; + PlayableBag& current_bag = playable_bags_.at(current_bag_index); + cartographer_ros_msgs::msg::BagfileProgress progress; + rosbag2_storage::SerializedBagMessage msg = current_bag.GetNextMessage(&progress); + const bool publish_progress = + current_bag.finished() || + node_->now() - bag_progress_time_map_[current_bag.bag_id()] >= + rclcpp::Duration(progress_pub_interval_, 0); + if (bag_progress_pub_->get_subscription_count() > 0 && publish_progress) { + progress.total_bagfiles = playable_bags_.size(); + if (current_bag.finished()) { + progress.processed_seconds = current_bag.duration_in_seconds(); + } + bag_progress_pub_->publish(progress); + bag_progress_time_map_[current_bag.bag_id()] = node_->now(); + } + CHECK_EQ(msg.time_stamp, next_message_queue_.top().message_timestamp.nanoseconds()); + next_message_queue_.pop(); + if (current_bag.IsMessageAvailable()) { + next_message_queue_.emplace( + BagMessageItem{current_bag.PeekMessageTime(), current_bag_index}); + } + + std::string topic_type; + for (auto topic_info : current_bag.bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == msg.topic_name){ + topic_type = topic_info.topic_metadata.type; + break; + } + } + + return std::make_tuple(std::move(msg), current_bag.bag_id(), topic_type, + !current_bag.IsMessageAvailable()); +} + +rclcpp::Time PlayableBagMultiplexer::PeekMessageTime() const { + CHECK(IsMessageAvailable()); + return next_message_queue_.top().message_timestamp; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/src/ros_log_sink.cpp similarity index 82% rename from cartographer_ros/cartographer_ros/ros_log_sink.cc rename to cartographer_ros/src/ros_log_sink.cpp index 8f6c385dd..d9b8ee2c1 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.cc +++ b/cartographer_ros/src/ros_log_sink.cpp @@ -18,11 +18,9 @@ #include #include -#include #include #include -#include #include "glog/log_severity.h" namespace cartographer_ros { @@ -45,24 +43,25 @@ void ScopedRosLogSink::send(const ::google::LogSeverity severity, const struct std::tm* const tm_time, const char* const message, const size_t message_len) { - (void)base_filename; + (void) base_filename; // TODO: remove unused arg ? + const std::string message_string = ::google::LogSink::ToString( severity, GetBasename(filename), line, tm_time, message, message_len); switch (severity) { case ::google::GLOG_INFO: - RCLCPP_INFO(rclcpp::get_logger("cartographer_ros"), "%s", message_string.c_str()); + RCLCPP_INFO_STREAM(logger_, message_string); break; case ::google::GLOG_WARNING: - RCLCPP_WARN(rclcpp::get_logger("cartographer_ros"), "%s", message_string.c_str()); + RCLCPP_WARN_STREAM(logger_, message_string); break; case ::google::GLOG_ERROR: - RCLCPP_ERROR(rclcpp::get_logger("cartographer_ros"), "%s", message_string.c_str()); + RCLCPP_ERROR_STREAM(logger_, message_string); break; case ::google::GLOG_FATAL: - RCLCPP_FATAL(rclcpp::get_logger("cartographer_ros"), "%s", message_string.c_str()); + RCLCPP_FATAL_STREAM(logger_, message_string); will_die_ = true; break; } diff --git a/cartographer_ros/cartographer_ros/ros_map.cc b/cartographer_ros/src/ros_map.cpp similarity index 78% rename from cartographer_ros/cartographer_ros/ros_map.cc rename to cartographer_ros/src/ros_map.cpp index 82f9776d6..a8a18d226 100644 --- a/cartographer_ros/cartographer_ros/ros_map.cc +++ b/cartographer_ros/src/ros_map.cpp @@ -18,12 +18,12 @@ namespace cartographer_ros { +// TODO: png ? void WritePgm(const ::cartographer::io::Image& image, const double resolution, ::cartographer::io::FileWriter* file_writer) { - const std::string header = "P5\n# Cartographer map; " + - std::to_string(resolution) + " m/pixel\n" + - std::to_string(image.width()) + " " + - std::to_string(image.height()) + "\n255\n"; + const std::string header = + "P5\n# Cartographer map; " + std::to_string(resolution) + " m/pixel\n" + + std::to_string(image.width()) + " " + std::to_string(image.height()) + "\n255\n"; file_writer->Write(header.data(), header.size()); for (int y = 0; y < image.height(); ++y) { for (int x = 0; x < image.width(); ++x) { @@ -38,10 +38,10 @@ void WriteYaml(const double resolution, const Eigen::Vector2d& origin, ::cartographer::io::FileWriter* file_writer) { // Magic constants taken directly from ros map_saver code: // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 + // TODO: check ROS2 map format const std::string output = - "image: " + pgm_filename + "\n" + - "resolution: " + std::to_string(resolution) + "\n" + "origin: [" + - std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + + "image: " + pgm_filename + "\n" + "resolution: " + std::to_string(resolution) + "\n" + + "origin: [" + std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"; file_writer->Write(output.data(), output.size()); } diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/src/ros_map_writing_points_processor.cpp similarity index 92% rename from cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc rename to cartographer_ros/src/ros_map_writing_points_processor.cpp index c25cb3b39..25c0e8312 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/src/ros_map_writing_points_processor.cpp @@ -16,7 +16,7 @@ #include "cartographer_ros/ros_map_writing_points_processor.h" -#include "cartographer/common/make_unique.h" +#include "absl/memory/memory.h" #include "cartographer/io/image.h" #include "cartographer/io/probability_grid_points_processor.h" #include "cartographer_ros/ros_map.h" @@ -34,15 +34,15 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor( next_(next), file_writer_factory_(file_writer_factory), range_data_inserter_(range_data_inserter_options), - probability_grid_(::cartographer::io::CreateProbabilityGrid(resolution)) { -} + probability_grid_(::cartographer::io::CreateProbabilityGrid( + resolution, &conversion_tables_)) {} std::unique_ptr RosMapWritingPointsProcessor::FromDictionary( ::cartographer::io::FileWriterFactory file_writer_factory, ::cartographer::common::LuaParameterDictionary* const dictionary, ::cartographer::io::PointsProcessor* const next) { - return ::cartographer::common::make_unique( + return absl::make_unique( dictionary->GetDouble("resolution"), ::cartographer::mapping::CreateProbabilityGridRangeDataInserterOptions2D( dictionary->GetDictionary("range_data_inserter").get()), @@ -51,8 +51,9 @@ RosMapWritingPointsProcessor::FromDictionary( void RosMapWritingPointsProcessor::Process( std::unique_ptr<::cartographer::io::PointsBatch> batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &probability_grid_); + range_data_inserter_.Insert( + {batch->origin, ::cartographer::sensor::PointCloud(batch->points), {}}, + &probability_grid_); next_->Process(std::move(batch)); } diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/src/rosbag_validate_main.cpp similarity index 55% rename from cartographer_ros/cartographer_ros/rosbag_validate_main.cc rename to cartographer_ros/src/rosbag_validate_main.cpp index b05db39eb..69b7e5b36 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/src/rosbag_validate_main.cpp @@ -20,24 +20,24 @@ #include #include +#include "absl/memory/memory.h" #include "cartographer/common/histogram.h" -#include "cartographer/common/make_unique.h" #include "cartographer_ros/msg_conversion.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "nav_msgs/Odometry.h" -#include "ros/ros.h" -#include "ros/time.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_msgs/TFMessage.h" +#include "nav_msgs/msg/odometry.hpp" +#include +#include +#include +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/buffer.h" #include "urdf/model.h" +#include DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_bool(dump_timing, false, @@ -48,7 +48,7 @@ namespace cartographer_ros { namespace { struct FrameProperties { - ros::Time last_timestamp; + rclcpp::Time last_timestamp; std::string topic; std::vector time_deltas; std::unique_ptr timing_file; @@ -64,15 +64,12 @@ const double kMaxAverageAcceleration = 10.5; const double kMaxGapPointsData = 0.1; const double kMaxGapImuData = 0.05; const std::set kPointDataTypes = { - std::string( - ros::message_traits::DataType::value()), - std::string(ros::message_traits::DataType< - sensor_msgs::MultiEchoLaserScan>::value()), - std::string( - ros::message_traits::DataType::value())}; + std::string(rosidl_generator_traits::data_type()), + std::string(rosidl_generator_traits::data_type()), + std::string(rosidl_generator_traits::data_type())}; std::unique_ptr CreateTimingFile(const std::string& frame_id) { - auto timing_file = ::cartographer::common::make_unique( + auto timing_file = absl::make_unique( std::string("timing_") + frame_id + ".csv", std::ios_base::out); (*timing_file) @@ -95,14 +92,14 @@ std::unique_ptr CreateTimingFile(const std::string& frame_id) { return timing_file; } -void CheckImuMessage(const sensor_msgs::Imu& imu_message) { - auto linear_acceleration = ToEigen(imu_message.linear_acceleration); +void CheckImuMessage(const sensor_msgs::msg::Imu::ConstSharedPtr imu_message) { + auto linear_acceleration = ToEigen(imu_message->linear_acceleration); if (std::isnan(linear_acceleration.norm()) || linear_acceleration.norm() < kMinLinearAcceleration || linear_acceleration.norm() > kMaxLinearAcceleration) { LOG_FIRST_N(WARNING, 3) - << "frame_id " << imu_message.header.frame_id << " time " - << imu_message.header.stamp.toNSec() << ": IMU linear acceleration is " + << "frame_id " << imu_message->header.frame_id << " time " + << imu_message->header.stamp.nanosec << ": IMU linear acceleration is " << linear_acceleration.norm() << " m/s^2," << " expected is [" << kMinLinearAcceleration << ", " << kMaxLinearAcceleration << "] m/s^2." @@ -111,22 +108,26 @@ void CheckImuMessage(const sensor_msgs::Imu& imu_message) { } } -bool IsValidPose(const geometry_msgs::Pose& pose) { +bool IsValidPose(const geometry_msgs::msg::Pose& pose) { return ToRigid3d(pose).IsValid(); } -void CheckOdometryMessage(const nav_msgs::Odometry& message) { - const auto& pose = message.pose.pose; +void CheckOdometryMessage(nav_msgs::msg::Odometry::ConstSharedPtr message) { + const auto& pose = message->pose.pose; if (!IsValidPose(pose)) { - LOG_FIRST_N(ERROR, 3) << "frame_id " << message.header.frame_id << " time " - << message.header.stamp.toNSec() + LOG_FIRST_N(ERROR, 3) << "frame_id " << message->header.frame_id << " time " + << message->header.stamp.nanosec << ": Odometry pose is invalid." - << " pose " << pose; + << " pose.position.x: " << pose.position.x + << " pose.position.y: " << pose.position.y + << " pose.position.z: " << pose.position.z + //<< " pose.orientation Yaw: " << tf2::getYaw(pose.orientation) + ; } } -void CheckTfMessage(const tf2_msgs::TFMessage& message) { - for (const auto& transform : message.transforms) { +void CheckTfMessage(tf2_msgs::msg::TFMessage::ConstSharedPtr message) { + for (auto transform : message->transforms) { if (transform.header.frame_id == "map") { LOG_FIRST_N(ERROR, 1) << "Input contains transform message from frame_id \"" @@ -149,7 +150,7 @@ class RangeDataChecker { template void CheckMessage(const MessageType& message) { const std::string& frame_id = message.header.frame_id; - ros::Time current_time_stamp = message.header.stamp; + rclcpp::Time current_time_stamp = message.header.stamp; RangeChecksum current_checksum; cartographer::common::Time time_from, time_to; ReadRangeMessage(message, ¤t_checksum, &time_from, &time_to); @@ -190,7 +191,7 @@ class RangeDataChecker { LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id << "\" sends exactly the same range measurements multiple times. " - << "Range data at time " << current_time_stamp + << "Range data at time " << current_time_stamp.seconds() << " equals preceding data with " << current_checksum.first << " points."; } @@ -219,14 +220,15 @@ class RangeDataChecker { const cartographer::sensor::TimedPointCloud& point_cloud = std::get<0>(point_cloud_time).points; *to = std::get<1>(point_cloud_time); - *from = *to + cartographer::common::FromSeconds(point_cloud[0][3]); + *from = *to + cartographer::common::FromSeconds(point_cloud[0].time); Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); - for (const Eigen::Vector4f& point : point_cloud) { - points_sum += point; + for (const auto& point : point_cloud) { + points_sum.head<3>() += point.position; + points_sum[3] += point.time; } if (point_cloud.size() > 0) { - double first_point_relative_time = point_cloud[0][3]; - double last_point_relative_time = (*point_cloud.rbegin())[3]; + double first_point_relative_time = point_cloud[0].time; + double last_point_relative_time = (*point_cloud.rbegin()).time; if (first_point_relative_time > 0) { LOG_FIRST_N(ERROR, 1) << "Sensor with frame_id \"" << message.header.frame_id @@ -250,111 +252,151 @@ class RangeDataChecker { }; void Run(const std::string& bag_filename, const bool dump_timing) { - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); - rosbag::View view(bag); + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + rosbag2_storage::BagMetadata bag_metadata = bag_reader.get_metadata(); + + auto pcl2_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto laser_scan_serializer = rclcpp::Serialization(); + auto imu_serializer = rclcpp::Serialization(); + auto odom_serializer = rclcpp::Serialization(); + auto tf_serializer = rclcpp::Serialization(); std::map frame_id_to_properties; size_t message_index = 0; int num_imu_messages = 0; double sum_imu_acceleration = 0.; RangeDataChecker range_data_checker; - for (const rosbag::MessageInstance& message : view) { + while (bag_reader.has_next()) { + auto message = bag_reader.read_next(); ++message_index; std::string frame_id; - ros::Time time; - if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - CheckImuMessage(*msg); - num_imu_messages++; - sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm(); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - CheckOdometryMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - CheckTfMessage(*msg); - continue; - } else { - continue; + rclcpp::Time time; + bool is_data_from_sensor = true; + std::string topic_type; + for (auto topic_info : bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == message->topic_name){ + if (topic_info.topic_metadata.type == "sensor_msgs/msg/PointCloud2") { + topic_type = "sensor_msgs/msg/PointCloud2"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::PointCloud2::SharedPtr msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/MultiEchoLaserScan") { + topic_type = "sensor_msgs/msg/MultiEchoLaserScan"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/LaserScan") { + topic_type = "sensor_msgs/msg/LaserScan"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::LaserScan::SharedPtr msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/Imu") { + topic_type = "sensor_msgs/msg/Imu"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::Imu::SharedPtr msg = + std::make_shared(); + imu_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckImuMessage(msg); + num_imu_messages++; + sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm(); + } else if (topic_info.topic_metadata.type == "nav_msgs/msg/Odometry") { + topic_type = "nav_msgs/msg/Odometry"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + nav_msgs::msg::Odometry::SharedPtr msg = + std::make_shared(); + odom_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckOdometryMessage(msg); + } else if (topic_info.topic_metadata.type == "tf2_msgs/msg/TFMessage") { + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + tf2_msgs::msg::TFMessage::SharedPtr msg + = std::make_shared();; + tf_serializer.deserialize_message(&serialized_msg, msg.get()); + CheckTfMessage(msg); + is_data_from_sensor = false; + } else { + is_data_from_sensor = false;; + } + break; + } } - bool first_packet = false; - if (!frame_id_to_properties.count(frame_id)) { - frame_id_to_properties.emplace( - frame_id, - FrameProperties{time, message.getTopic(), std::vector(), - dump_timing ? CreateTimingFile(frame_id) : nullptr, - message.getDataType()}); - first_packet = true; - } + if (is_data_from_sensor) { + bool first_packet = false; + if (!frame_id_to_properties.count(frame_id)) { + frame_id_to_properties.emplace( + frame_id, + FrameProperties{time, message->topic_name, std::vector(), + dump_timing ? CreateTimingFile(frame_id) : nullptr, + topic_type}); + first_packet = true; + } - auto& entry = frame_id_to_properties.at(frame_id); - if (!first_packet) { - const double delta_t_sec = (time - entry.last_timestamp).toSec(); - if (delta_t_sec < 0) { - LOG_FIRST_N(ERROR, 3) - << "Sensor with frame_id \"" << frame_id - << "\" jumps backwards in time. Make sure that the bag " - "contains the data for each frame_id sorted by " - "header.stamp, i.e. the order in which they were " - "acquired from the sensor."; + auto& entry = frame_id_to_properties.at(frame_id); + if (!first_packet) { + const double delta_t_sec = (time - entry.last_timestamp).seconds(); + if (delta_t_sec <= 0) { + LOG_FIRST_N(ERROR, 3) + << "Sensor with frame_id \"" << frame_id + << "\" jumps backwards in time, i.e. timestamps are not strictly " + "increasing. Make sure that the bag contains the data for each " + "frame_id sorted by header.stamp, i.e. the order in which they " + "were acquired from the sensor."; + } + entry.time_deltas.push_back(delta_t_sec); } - entry.time_deltas.push_back(delta_t_sec); - } - if (entry.topic != message.getTopic()) { - LOG_FIRST_N(ERROR, 3) - << "frame_id \"" << frame_id - << "\" is send on multiple topics. It was seen at least on " - << entry.topic << " and " << message.getTopic(); - } - entry.last_timestamp = time; + if (entry.topic != message->topic_name) { + LOG_FIRST_N(ERROR, 3) + << "frame_id \"" << frame_id + << "\" is send on multiple topics. It was seen at least on " + << entry.topic << " and " << message->topic_name; + } + entry.last_timestamp = time; - if (dump_timing) { - CHECK(entry.timing_file != nullptr); - (*entry.timing_file) << message_index << "\t" - << message.getTime().toNSec() << "\t" - << time.toNSec() << std::endl; - } + if (dump_timing) { + CHECK(entry.timing_file != nullptr); + (*entry.timing_file) << message_index << "\t" + << message->time_stamp << "\t" + << time.nanoseconds() << std::endl; + } - double duration_serialization_sensor = (time - message.getTime()).toSec(); - if (std::abs(duration_serialization_sensor) > - kTimeDeltaSerializationSensorWarning) { - std::stringstream stream; - stream << "frame_id \"" << frame_id << "\" on topic " - << message.getTopic() << " has serialization time " - << message.getTime() << " but sensor time " << time - << " differing by " << duration_serialization_sensor << " s."; + double duration_serialization_sensor = (time.nanoseconds() - message->time_stamp)/1e9; if (std::abs(duration_serialization_sensor) > - kTimeDeltaSerializationSensorError) { - LOG_FIRST_N(ERROR, 3) << stream.str(); - } else { - LOG_FIRST_N(WARNING, 1) << stream.str(); + kTimeDeltaSerializationSensorWarning) { + std::stringstream stream; + stream << "frame_id \"" << frame_id << "\" on topic " + << message->topic_name << " has serialization time " + << message->time_stamp << " but sensor time " << time.nanoseconds() + << " differing by " << duration_serialization_sensor << " s."; + if (std::abs(duration_serialization_sensor) > + kTimeDeltaSerializationSensorError) { + LOG_FIRST_N(ERROR, 3) << stream.str(); + } else { + LOG_FIRST_N(WARNING, 1) << stream.str(); + } } } } - bag.close(); + //end while range_data_checker.PrintReport(); if (num_imu_messages > 0) { @@ -384,7 +426,7 @@ void Run(const std::string& bag_filename, const bool dump_timing) { << " s, recommended is [0.0005, 0.05] s with no jitter."; } if (frame_properties.data_type == - ros::message_traits::DataType::value() && + rosidl_generator_traits::data_type() && max_time_delta > kMaxGapImuData) { LOG(ERROR) << "IMU data (frame_id: \"" << entry_pair.first << "\") has a large gap, largest is " << max_time_delta @@ -415,9 +457,13 @@ void Run(const std::string& bag_filename, const bool dump_timing) { } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/src/sensor_bridge.cpp similarity index 81% rename from cartographer_ros/cartographer_ros/sensor_bridge.cc rename to cartographer_ros/src/sensor_bridge.cpp index 284851404..d1e4b18c9 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/src/sensor_bridge.cpp @@ -16,7 +16,7 @@ #include "cartographer_ros/sensor_bridge.h" -#include "cartographer/common/make_unique.h" +#include "absl/memory/memory.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" @@ -56,7 +56,7 @@ std::unique_ptr SensorBridge::ToOdometryData( if (sensor_to_tracking == nullptr) { return nullptr; } - return carto::common::make_unique( + return absl::make_unique( carto::sensor::OdometryData{ time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()}); } @@ -77,8 +77,8 @@ void SensorBridge::HandleNavSatFixMessage( const carto::common::Time time = FromRos(msg->header.stamp); if (msg->status.status == sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX) { trajectory_builder_->AddSensorData( - sensor_id, carto::sensor::FixedFramePoseData{ - time, carto::common::optional()}); + sensor_id, + carto::sensor::FixedFramePoseData{time, absl::optional()}); return; } @@ -90,18 +90,28 @@ void SensorBridge::HandleNavSatFixMessage( } trajectory_builder_->AddSensorData( - sensor_id, - carto::sensor::FixedFramePoseData{ - time, carto::common::optional(Rigid3d::Translation( - ecef_to_local_frame_.value() * - LatLongAltToEcef(msg->latitude, msg->longitude, - msg->altitude)))}); + sensor_id, carto::sensor::FixedFramePoseData{ + time, absl::optional(Rigid3d::Translation( + ecef_to_local_frame_.value() * + LatLongAltToEcef(msg->latitude, msg->longitude, + msg->altitude)))}); } void SensorBridge::HandleLandmarkMessage( const std::string& sensor_id, const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg) { - trajectory_builder_->AddSensorData(sensor_id, ToLandmarkData(*msg)); + auto landmark_data = ToLandmarkData(*msg); + + auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking( + landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id)); + if (tracking_from_landmark_sensor != nullptr) { + for (auto& observation : landmark_data.landmark_observations) { + observation.landmark_to_tracking_transform = + *tracking_from_landmark_sensor * + observation.landmark_to_tracking_transform; + } + } + trajectory_builder_->AddSensorData(sensor_id, landmark_data); } std::unique_ptr SensorBridge::ToImuData( @@ -127,11 +137,9 @@ std::unique_ptr SensorBridge::ToImuData( << "The IMU frame must be colocated with the tracking frame. " "Transforming linear acceleration into the tracking frame will " "otherwise be imprecise."; - return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>( - ::cartographer::sensor::ImuData{ - time, - sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), - sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); + return absl::make_unique(carto::sensor::ImuData{ + time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), + sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); } void SensorBridge::HandleImuMessage(const std::string& sensor_id, @@ -165,14 +173,10 @@ void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandlePointCloud2Message( const std::string& sensor_id, const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*msg, pcl_point_cloud); - carto::sensor::TimedPointCloud point_cloud; - for (const auto& point : pcl_point_cloud) { - point_cloud.emplace_back(point.x, point.y, point.z, 0.f); - } - HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - point_cloud); + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points); } const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } @@ -184,7 +188,7 @@ void SensorBridge::HandleLaserScan( if (points.points.empty()) { return; } - CHECK_LE(points.points.back()[3], 0); + CHECK_LE(points.points.back().time, 0.f); // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = @@ -196,7 +200,7 @@ void SensorBridge::HandleLaserScan( if (start_index == end_index) { continue; } - const double time_to_subdivision_end = subdivision.back()[3]; + const double time_to_subdivision_end = subdivision.back().time; // `subdivision_time` is the end of the measurement so sensor::Collator will // send all other sensor data first. const carto::common::Time subdivision_time = @@ -211,10 +215,10 @@ void SensorBridge::HandleLaserScan( continue; } sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time; - for (Eigen::Vector4f& point : subdivision) { - point[3] -= time_to_subdivision_end; + for (auto& point : subdivision) { + point.time -= time_to_subdivision_end; } - CHECK_EQ(subdivision.back()[3], 0); + CHECK_EQ(subdivision.back().time, 0.f); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } } @@ -222,6 +226,13 @@ void SensorBridge::HandleLaserScan( void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { + if (!ranges.empty()) { + CHECK_LE(ranges.back().time, 0.f); + } + + // This was added to get rid of the TimedPointCloudData warning for a missing argument + std::vector intensities_; + const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { @@ -229,7 +240,7 @@ void SensorBridge::HandleRangefinder( sensor_id, carto::sensor::TimedPointCloudData{ time, sensor_to_tracking->translation().cast(), carto::sensor::TransformTimedPointCloud( - ranges, sensor_to_tracking->cast())}); + ranges, sensor_to_tracking->cast()), intensities_}); } } diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/src/submap.cpp similarity index 62% rename from cartographer_ros/cartographer_ros/submap.cc rename to cartographer_ros/src/submap.cpp index 86729b625..40fa94f46 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/src/submap.cpp @@ -16,7 +16,7 @@ #include "cartographer_ros/submap.h" -#include "cartographer/common/make_unique.h" +#include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/msg_conversion.h" @@ -26,8 +26,29 @@ namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( - std::shared_ptr<::cartographer_ros_msgs::srv::SubmapQuery::Response> result){ - auto response = ::cartographer::common::make_unique<::cartographer::io::SubmapTextures>(); + const ::cartographer::mapping::SubmapId& submap_id, + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout) +{ + auto request = std::make_shared(); + request->trajectory_id = submap_id.trajectory_id; + request->submap_index = submap_id.submap_index; + auto future_result = client->async_send_request(request); + + if (callback_group_executor->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return nullptr; + } + auto result = future_result.get(); + + if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || + result->textures.empty()) { + return nullptr; + } + + auto response = absl::make_unique<::cartographer::io::SubmapTextures>(); response->version = result->submap_version; for (const auto& texture : result->textures) { const std::string compressed_cells(texture.cells.begin(), diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/src/tf_bridge.cpp similarity index 51% rename from cartographer_ros/cartographer_ros/tf_bridge.cc rename to cartographer_ros/src/tf_bridge.cpp index 4443b5152..dcd6225a8 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.cc +++ b/cartographer_ros/src/tf_bridge.cpp @@ -14,10 +14,10 @@ * limitations under the License. */ -#include "cartographer/common/make_unique.h" +#include "cartographer_ros/tf_bridge.h" +#include "absl/memory/memory.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/tf_bridge.h" namespace cartographer_ros { @@ -34,35 +34,21 @@ std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( tf2::Duration timeout(tf2::durationFromSec(lookup_transform_timeout_sec_)); std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; try { - const ::builtin_interfaces::msg::Time latest_tf_time = + const rclcpp::Time latest_tf_time = buffer_ - ->lookupTransform(tracking_frame_, frame_id, tf2::TimePointZero, + ->lookupTransform(tracking_frame_, frame_id, ::rclcpp::Time(0.), timeout) .header.stamp; - const ::builtin_interfaces::msg::Time requested_time = ToRos(time); - std::chrono::time_point converted{std::chrono::nanoseconds{requested_time.sec*1000000000LL+requested_time.nanosec}}; - std::chrono::system_clock::time_point recovered = std::chrono::time_point_cast(converted); + const rclcpp::Time requested_time = ToRos(time); - std::chrono::time_point converted_tf_time{std::chrono::nanoseconds{latest_tf_time.sec*1000000000LL+latest_tf_time.nanosec}}; - std::chrono::system_clock::time_point recovered_tf_time = std::chrono::time_point_cast(converted_tf_time); - - if (recovered_tf_time >= recovered) { + if (latest_tf_time >= requested_time) { // We already have newer data, so we do not wait. Otherwise, we would wait // for the full 'timeout' even if we ask for data that is too old. timeout = tf2::durationFromSec(0.0); } - - // TODO(clalancette): We are currently having some problems where the - // conversions from tf2 time to cartographer time is rounding. In turn - // this causes tf2 to complain about extrapolating into the future. For - // right now, just always ask for the data "now", which works around the - // problem. We'll need to address this for real in the future. - //return ::cartographer::common::make_unique< - // ::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform( - // tracking_frame_, frame_id, recovered, timeout))); - return ::cartographer::common::make_unique< - ::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform( - tracking_frame_, frame_id, recovered, timeout))); + return absl::make_unique<::cartographer::transform::Rigid3d>( + ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id, + requested_time, timeout))); } catch (const tf2::TransformException& ex) { LOG(WARNING) << ex.what(); } diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/src/time_conversion.cpp similarity index 70% rename from cartographer_ros/cartographer_ros/time_conversion.cc rename to cartographer_ros/src/time_conversion.cpp index 059c73a03..c852aab41 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.cc +++ b/cartographer_ros/src/time_conversion.cpp @@ -21,28 +21,24 @@ namespace cartographer_ros { -builtin_interfaces::msg::Time ToRos(::cartographer::common::Time time) { +rclcpp::Time ToRos(::cartographer::common::Time time) { int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); int64_t ns_since_unix_epoch = (uts_timestamp - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) * 100ll; - builtin_interfaces::msg::Time ros_time; - ros_time.sec = static_cast(ns_since_unix_epoch / 1000000000); - ros_time.nanosec = ns_since_unix_epoch % 1000000000; + rclcpp::Time ros_time(ns_since_unix_epoch, rcl_clock_type_t::RCL_ROS_TIME); return ros_time; } // TODO(pedrofernandez): Write test. -::cartographer::common::Time FromRos(const builtin_interfaces::msg::Time& time) { +::cartographer::common::Time FromRos(const rclcpp::Time& time) { // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", // exactly 719162 days before the Unix epoch. return ::cartographer::common::FromUniversal( - (time.sec + - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) * - 10000000ll + - (time.nanosec + 50) / 100); // + 50 to get the rounding correct. + cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll + + (time.nanoseconds() + 50) / 100); // + 50 to get the rounding correct. } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/src/time_conversion_test.cpp similarity index 97% rename from cartographer_ros/cartographer_ros/time_conversion_test.cc rename to cartographer_ros/src/time_conversion_test.cpp index 3b2d57230..13337fb3a 100644 --- a/cartographer_ros/cartographer_ros/time_conversion_test.cc +++ b/cartographer_ros/src/time_conversion_test.cpp @@ -29,7 +29,7 @@ TEST(TimeConversion, testToRos) { std::vector values = {0, 1469091375, 1466481821, 1462101382, 1468238899}; for (int64_t seconds_since_epoch : values) { - ::ros::Time ros_now; + builtin_interfaces::msg::Time ros_now; ros_now.fromSec(seconds_since_epoch); ::cartographer::common::Time cartographer_now( ::cartographer::common::FromSeconds( diff --git a/cartographer_ros/src/trajectory_options.cpp b/cartographer_ros/src/trajectory_options.cpp new file mode 100644 index 000000000..143f5b22e --- /dev/null +++ b/cartographer_ros/src/trajectory_options.cpp @@ -0,0 +1,82 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * 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. + */ + +#include "cartographer_ros/trajectory_options.h" + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/time_conversion.h" +#include "glog/logging.h" + +namespace cartographer_ros { + +namespace { + +void CheckTrajectoryOptions(const TrajectoryOptions& options) { + CHECK_GE(options.num_subdivisions_per_laser_scan, 1); + CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans + + options.num_point_clouds, + 1) + << "Configuration error: 'num_laser_scans', " + "'num_multi_echo_laser_scans' and 'num_point_clouds' are " + "all zero, but at least one is required."; +} + +} // namespace + +TrajectoryOptions CreateTrajectoryOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + TrajectoryOptions options; + options.trajectory_builder_options = + ::cartographer::mapping::CreateTrajectoryBuilderOptions( + lua_parameter_dictionary->GetDictionary("trajectory_builder").get()); + options.tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + options.published_frame = + lua_parameter_dictionary->GetString("published_frame"); + options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); + options.provide_odom_frame = + lua_parameter_dictionary->GetBool("provide_odom_frame"); + options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); + options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); + options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks"); + options.publish_frame_projected_to_2d = + lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d"); + options.num_laser_scans = + lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); + options.num_multi_echo_laser_scans = + lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans"); + options.num_subdivisions_per_laser_scan = + lua_parameter_dictionary->GetNonNegativeInt( + "num_subdivisions_per_laser_scan"); + options.num_point_clouds = + lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds"); + options.rangefinder_sampling_ratio = + lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio"); + options.odometry_sampling_ratio = + lua_parameter_dictionary->GetDouble("odometry_sampling_ratio"); + options.fixed_frame_pose_sampling_ratio = + lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio"); + options.imu_sampling_ratio = + lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); + options.landmarks_sampling_ratio = + lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio"); + CheckTrajectoryOptions(options); + return options; +} +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/src/urdf_reader.cpp similarity index 82% rename from cartographer_ros/cartographer_ros/urdf_reader.cc rename to cartographer_ros/src/urdf_reader.cpp index e389e18d7..d2380cd40 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.cc +++ b/cartographer_ros/src/urdf_reader.cpp @@ -24,17 +24,13 @@ namespace cartographer_ros { -std::vector ReadStaticTransformsFromUrdf( - const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, std::shared_ptr tf_buffer) { urdf::Model model; CHECK(model.initFile(urdf_filename)); -#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS std::vector links; -#else - std::vector > links; -#endif model.getLinks(links); - std::vector transforms; + std::vector transforms; for (const auto& link : links) { if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { continue; @@ -42,7 +38,7 @@ std::vector ReadStaticTransformsFromUrdf( const urdf::Pose& pose = link->parent_joint->parent_to_joint_origin_transform; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; transform.transform = ToGeometryMsgTransform(cartographer::transform::Rigid3d( Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), diff --git a/cartographer_ros/urdf/mir-100.urdf b/cartographer_ros/urdf/mir-100.urdf new file mode 100644 index 000000000..1b2e87c83 --- /dev/null +++ b/cartographer_ros/urdf/mir-100.urdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cartographer_ros_msgs/CHANGELOG.rst b/cartographer_ros_msgs/CHANGELOG.rst index 140973a21..9f73069c7 100644 --- a/cartographer_ros_msgs/CHANGELOG.rst +++ b/cartographer_ros_msgs/CHANGELOG.rst @@ -2,18 +2,6 @@ Changelog for package cartographer_ros_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.0.9004 (2021-10-28) ---------------------- - -1.0.9003 (2021-01-25) ---------------------- - -1.0.9002 (2020-10-30) ---------------------- - -1.0.9001 (2020-02-06) ---------------------- - 0.3.0 (2017-11-23) ------------------ * https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0 diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index ada9ebd9d..a60f711ef 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -16,36 +16,46 @@ cmake_minimum_required(VERSION 3.5) project(cartographer_ros_msgs) -# Default to C++14 +# Default to C++14. if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # we dont use add_compile_options with pedantic in message packages - # because the Python C extensions dont comply with it + # We don't use add_compile_options with pedantic in message packages + # because the Python C extensions don't comply with it. set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") endif() find_package(ament_cmake REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) set(msg_files + "msg/BagfileProgress.msg" + "msg/HistogramBucket.msg" "msg/LandmarkEntry.msg" "msg/LandmarkList.msg" - "msg/SensorTopics.msg" + "msg/MetricFamily.msg" + "msg/MetricLabel.msg" + "msg/Metric.msg" "msg/StatusCode.msg" "msg/StatusResponse.msg" "msg/SubmapEntry.msg" "msg/SubmapList.msg" "msg/SubmapTexture.msg" - "msg/TrajectoryOptions.msg" + "msg/TrajectoryStates.msg" ) + set(srv_files "srv/FinishTrajectory.srv" + "srv/GetTrajectoryStates.srv" + "srv/ReadMetrics.srv" "srv/StartTrajectory.srv" + "srv/TrajectoryQuery.srv" "srv/SubmapQuery.srv" "srv/WriteState.srv" ) diff --git a/cartographer_ros_msgs/msg/BagfileProgress.msg b/cartographer_ros_msgs/msg/BagfileProgress.msg new file mode 100644 index 000000000..88a810e5f --- /dev/null +++ b/cartographer_ros_msgs/msg/BagfileProgress.msg @@ -0,0 +1,23 @@ +# +# 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. + + +# Contains general information about the bagfiles processing progress + +string current_bagfile_name +uint32 current_bagfile_id +uint32 total_bagfiles +uint32 total_messages +uint32 processed_messages +float32 total_seconds +float32 processed_seconds diff --git a/cartographer_ros_msgs/msg/SensorTopics.msg b/cartographer_ros_msgs/msg/HistogramBucket.msg similarity index 72% rename from cartographer_ros_msgs/msg/SensorTopics.msg rename to cartographer_ros_msgs/msg/HistogramBucket.msg index 1db96864d..09cd5f784 100644 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ b/cartographer_ros_msgs/msg/HistogramBucket.msg @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# 2018 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,10 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -string laser_scan_topic -string multi_echo_laser_scan_topic -string point_cloud2_topic -string imu_topic -string odometry_topic -string nav_sat_fix_topic -string landmark_topic +# A histogram bucket counts values x for which: +# previous_boundary < x <= bucket_boundary +# holds. +float64 bucket_boundary +float64 count diff --git a/cartographer_ros_msgs/msg/LandmarkList.msg b/cartographer_ros_msgs/msg/LandmarkList.msg index 5c5bd2de0..a8e25efed 100644 --- a/cartographer_ros_msgs/msg/LandmarkList.msg +++ b/cartographer_ros_msgs/msg/LandmarkList.msg @@ -13,4 +13,4 @@ # limitations under the License. std_msgs/Header header -cartographer_ros_msgs/LandmarkEntry[] landmark +cartographer_ros_msgs/LandmarkEntry[] landmarks diff --git a/cartographer_ros_msgs/msg/Metric.msg b/cartographer_ros_msgs/msg/Metric.msg new file mode 100644 index 000000000..6d886d667 --- /dev/null +++ b/cartographer_ros_msgs/msg/Metric.msg @@ -0,0 +1,26 @@ +# 2018 The Cartographer Authors +# +# 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. + +uint8 TYPE_COUNTER=0 +uint8 TYPE_GAUGE=1 +uint8 TYPE_HISTOGRAM=2 + +uint8 type +cartographer_ros_msgs/MetricLabel[] labels + +# TYPE_COUNTER or TYPE_GAUGE +float64 value + +# TYPE_HISTOGRAM +cartographer_ros_msgs/HistogramBucket[] counts_by_bucket diff --git a/cartographer_ros_msgs/msg/MetricFamily.msg b/cartographer_ros_msgs/msg/MetricFamily.msg new file mode 100644 index 000000000..3996abe7d --- /dev/null +++ b/cartographer_ros_msgs/msg/MetricFamily.msg @@ -0,0 +1,17 @@ +# 2018 The Cartographer Authors +# +# 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. + +string name +string description +cartographer_ros_msgs/Metric[] metrics diff --git a/cartographer_ros_msgs/msg/MetricLabel.msg b/cartographer_ros_msgs/msg/MetricLabel.msg new file mode 100644 index 000000000..12eb1f09b --- /dev/null +++ b/cartographer_ros_msgs/msg/MetricLabel.msg @@ -0,0 +1,16 @@ +# 2018 The Cartographer Authors +# +# 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. + +string key +string value diff --git a/cartographer_ros_msgs/msg/SubmapEntry.msg b/cartographer_ros_msgs/msg/SubmapEntry.msg index 4ea51f935..2a38f7c58 100644 --- a/cartographer_ros_msgs/msg/SubmapEntry.msg +++ b/cartographer_ros_msgs/msg/SubmapEntry.msg @@ -16,3 +16,4 @@ int32 trajectory_id int32 submap_index int32 submap_version geometry_msgs/Pose pose +bool is_frozen diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg deleted file mode 100644 index b41810ce5..000000000 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ /dev/null @@ -1,35 +0,0 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - -string tracking_frame -string published_frame -string odom_frame -bool provide_odom_frame -bool use_odometry -bool use_nav_sat -bool use_landmarks -bool publish_frame_projected_to_2d -int32 num_laser_scans -int32 num_multi_echo_laser_scans -int32 num_subdivisions_per_laser_scan -int32 num_point_clouds -float64 rangefinder_sampling_ratio -float64 odometry_sampling_ratio -float64 fixed_frame_pose_sampling_ratio -float64 imu_sampling_ratio -float64 landmarks_sampling_ratio - -# This is a binary-encoded -# 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto. -string trajectory_builder_options_proto diff --git a/cartographer_ros_msgs/msg/TrajectoryStates.msg b/cartographer_ros_msgs/msg/TrajectoryStates.msg new file mode 100644 index 000000000..8f3a43b2f --- /dev/null +++ b/cartographer_ros_msgs/msg/TrajectoryStates.msg @@ -0,0 +1,22 @@ +# Copyright 2018 The Cartographer Authors +# +# 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. + +uint8 ACTIVE = 0 +uint8 FINISHED = 1 +uint8 FROZEN = 2 +uint8 DELETED = 3 + +std_msgs/Header header +int32[] trajectory_id +uint8[] trajectory_state diff --git a/cartographer_ros_msgs/package.xml b/cartographer_ros_msgs/package.xml index 934f2eccc..1cbf10141 100644 --- a/cartographer_ros_msgs/package.xml +++ b/cartographer_ros_msgs/package.xml @@ -17,27 +17,21 @@ cartographer_ros_msgs - - - 1.0.9004 + 1.0.0 ROS messages for the cartographer_ros package. - Chris Lalancette - Pyo + + The Cartographer Authors + Apache 2.0 - https://github.com/ros2/cartographer_ros + + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors - Darby Lim - ament_cmake rosidl_default_generators diff --git a/cartographer_ros_msgs/srv/GetTrajectoryStates.srv b/cartographer_ros_msgs/srv/GetTrajectoryStates.srv new file mode 100644 index 000000000..5ae7a90eb --- /dev/null +++ b/cartographer_ros_msgs/srv/GetTrajectoryStates.srv @@ -0,0 +1,18 @@ +# Copyright 2018 The Cartographer Authors +# +# 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. + + +--- +cartographer_ros_msgs/StatusResponse status +cartographer_ros_msgs/TrajectoryStates trajectory_states diff --git a/cartographer_ros_msgs/srv/ReadMetrics.srv b/cartographer_ros_msgs/srv/ReadMetrics.srv new file mode 100644 index 000000000..d6be66f07 --- /dev/null +++ b/cartographer_ros_msgs/srv/ReadMetrics.srv @@ -0,0 +1,18 @@ +# Copyright 2018 The Cartographer Authors +# +# 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. + +--- +cartographer_ros_msgs/StatusResponse status +cartographer_ros_msgs/MetricFamily[] metric_families +builtin_interfaces/Time timestamp diff --git a/cartographer_ros_msgs/srv/StartTrajectory.srv b/cartographer_ros_msgs/srv/StartTrajectory.srv index 98e245ade..569663548 100644 --- a/cartographer_ros_msgs/srv/StartTrajectory.srv +++ b/cartographer_ros_msgs/srv/StartTrajectory.srv @@ -12,8 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -cartographer_ros_msgs/TrajectoryOptions options -cartographer_ros_msgs/SensorTopics topics +string configuration_directory +string configuration_basename +bool use_initial_pose +geometry_msgs/Pose initial_pose +int32 relative_to_trajectory_id --- cartographer_ros_msgs/StatusResponse status int32 trajectory_id diff --git a/cartographer_ros_msgs/srv/TrajectoryQuery.srv b/cartographer_ros_msgs/srv/TrajectoryQuery.srv new file mode 100644 index 000000000..ca5da3b94 --- /dev/null +++ b/cartographer_ros_msgs/srv/TrajectoryQuery.srv @@ -0,0 +1,18 @@ +# Copyright 2019 The Cartographer Authors +# +# 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. + +int32 trajectory_id +--- +cartographer_ros_msgs/StatusResponse status +geometry_msgs/PoseStamped[] trajectory diff --git a/cartographer_ros_msgs/srv/WriteState.srv b/cartographer_ros_msgs/srv/WriteState.srv index cb7c1c884..6b401965b 100644 --- a/cartographer_ros_msgs/srv/WriteState.srv +++ b/cartographer_ros_msgs/srv/WriteState.srv @@ -13,5 +13,6 @@ # limitations under the License. string filename +bool include_unfinished_submaps --- cartographer_ros_msgs/StatusResponse status diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 47d8a9f81..957e855f1 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -1,4 +1,5 @@ # Copyright 2016 The Cartographer Authors +# Copyright 2022 Wyca Robotics (for the ROS2 conversion) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,88 +13,122 @@ # See the License for the specific language governing permissions and # limitations under the License. -cmake_minimum_required(VERSION 2.8.12) # Ships with Ubuntu 14.04 (Trusty) +cmake_minimum_required(VERSION 3.5) project(cartographer_rviz) +set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) -set(PACKAGE_DEPENDENCIES - cartographer_ros - cartographer_ros_msgs - eigen_conversions - message_runtime - roscpp - roslib - rviz -) +find_package(ament_cmake REQUIRED) + +# Default to C++17 +set(CMAKE_CXX_STANDARD 17) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() +set(CMAKE_AUTOMOC ON) + +find_package(Boost REQUIRED COMPONENTS system iostreams) find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +set(BUILD_SHARED_LIBS OFF) +option(BUILD_GRPC "build features that require Cartographer gRPC support" false) google_initialize_cartographer_project() find_package(Eigen3 REQUIRED) -find_package(Boost REQUIRED COMPONENTS system iostreams) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) - -catkin_package( - CATKIN_DEPENDS - message_runtime - ${PACKAGE_DEPENDENCIES} - INCLUDE_DIRS "." -) - -file(GLOB_RECURSE ALL_SRCS "*.cc" "*.h") -set(CMAKE_AUTOMOC ON) -add_library(${PROJECT_NAME} ${ALL_SRCS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) - -if(rviz_QT_VERSION VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Widgets) - include_directories(${Qt5Widgets_INCLUDE_DIRS}) -endif() -add_definitions(-DQT_NO_KEYWORDS) - -# Add the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ +find_package(cartographer_ros REQUIRED) +find_package(cartographer_ros_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz2 REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(rviz_common REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rcutils REQUIRED) + +set(CMAKE_THREAD_PREFER_PTHREAD TRUE) +set(THREADS_PREFER_PTHREAD_FLAG TRUE) +find_package(Threads REQUIRED) + +set(rviz_plugins_headers_to_moc + include/cartographer_rviz/drawable_submap.h + include/cartographer_rviz/ogre_slice.h + include/cartographer_rviz/submaps_display.h ) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${EIGEN3_INCLUDE_DIR}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${Boost_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) +include_directories( + include + ) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +add_library(${PROJECT_NAME} SHARED + src/drawable_submap.cpp + src/ogre_slice.cpp + src/submaps_display.cpp + ${rviz_plugins_headers_to_moc} + ) -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) +set(dependencies + rclcpp + cartographer + cartographer_ros + cartographer_ros_msgs + rviz2 + rviz_common + pcl_conversions + rcutils + ) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) +target_include_directories(${PROJECT_NAME} PUBLIC + ${PCL_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} +) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_link_libraries(${PROJECT_NAME} + cartographer + ${PCL_LIBRARIES} + ${QT_LIBRARIES} + ${EIGEN3_LIBRARIES} + ${Boost_LIBRARIES} + rviz_common::rviz_common + ) + + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts") + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) -install(FILES rviz_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY include/ + DESTINATION include/ ) -install(DIRECTORY ogre_media - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ${dependencies} ) + +ament_package() diff --git a/cartographer_rviz/COLCON_IGNORE b/cartographer_rviz/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/include/cartographer_rviz/drawable_submap.h similarity index 69% rename from cartographer_rviz/cartographer_rviz/drawable_submap.h rename to cartographer_rviz/include/cartographer_rviz/drawable_submap.h index df8cc18de..fe9d5ba04 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/include/cartographer_rviz/drawable_submap.h @@ -19,50 +19,52 @@ #include #include - +#include #include "Eigen/Core" #include "Eigen/Geometry" #include "OgreSceneManager.h" #include "OgreSceneNode.h" -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/submap.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" #include "cartographer_rviz/ogre_slice.h" -#include "ros/ros.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/axes.h" -#include "rviz/ogre_helpers/movable_text.h" -#include "rviz/properties/bool_property.h" +#include +#include +#include +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/movable_text.hpp" +#include namespace cartographer_rviz { // Contains all the information needed to render a submap onto the final // texture representing the whole map. -class DrawableSubmap : public QObject { +class DrawableSubmap : public QObject{ Q_OBJECT public: DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, - ::rviz::DisplayContext* display_context, - Ogre::SceneNode* map_node, ::rviz::Property* submap_category, - bool visible, float pose_axes_length, float pose_axes_radius); + ::rviz_common::DisplayContext* display_context, + Ogre::SceneNode* map_node, ::rviz_common::properties::Property* submap_category, + bool visible, const bool pose_axes_visible, + float pose_axes_length, float pose_axes_radius); ~DrawableSubmap() override; DrawableSubmap(const DrawableSubmap&) = delete; DrawableSubmap& operator=(const DrawableSubmap&) = delete; // Updates the 'metadata' for this submap. If necessary, the next call to // MaybeFetchTexture() will fetch a new submap texture. - void Update(const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata); + void Update(const ::std_msgs::msg::Header& header, + const ::cartographer_ros_msgs::msg::SubmapEntry& metadata); // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. - bool MaybeFetchTexture(ros::ServiceClient* client); + bool MaybeFetchTexture(rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor); // Returns whether an RPC is in progress. bool QueryInProgress(); @@ -83,6 +85,10 @@ class DrawableSubmap : public QObject { void set_visibility(const bool visibility) { visibility_->setBool(visibility); } + void set_pose_markers_visibility(const bool visibility) { + pose_axes_visible_ = visibility; + TogglePoseMarkerVisibility(); + } Q_SIGNALS: // RPC request succeeded. @@ -92,18 +98,20 @@ class DrawableSubmap : public QObject { // Callback when an rpc request succeeded. void UpdateSceneNode(); void ToggleVisibility(); + void TogglePoseMarkerVisibility(); private: const ::cartographer::mapping::SubmapId id_; - ::cartographer::common::Mutex mutex_; - ::rviz::DisplayContext* const display_context_; + absl::Mutex mutex_; + ::rviz_common::DisplayContext* const display_context_; Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const submap_id_text_node_; std::vector> ogre_slices_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); - ::rviz::Axes pose_axes_; - ::rviz::MovableText submap_id_text_; + ::rviz_rendering::Axes pose_axes_; + bool pose_axes_visible_; + ::rviz_rendering::MovableText submap_id_text_; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ GUARDED_BY(mutex_) = false; int metadata_version_ GUARDED_BY(mutex_) = -1; @@ -111,7 +119,7 @@ class DrawableSubmap : public QObject { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_ GUARDED_BY(mutex_); float current_alpha_ = 0.f; - std::unique_ptr<::rviz::BoolProperty> visibility_; + std::unique_ptr<::rviz_common::properties::BoolProperty> visibility_; }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.h b/cartographer_rviz/include/cartographer_rviz/ogre_slice.h similarity index 100% rename from cartographer_rviz/cartographer_rviz/ogre_slice.h rename to cartographer_rviz/include/cartographer_rviz/ogre_slice.h diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/include/cartographer_rviz/submaps_display.h similarity index 55% rename from cartographer_rviz/cartographer_rviz/submaps_display.h rename to cartographer_rviz/include/cartographer_rviz/submaps_display.h index 9b68b3996..4b128aabd 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/include/cartographer_rviz/submaps_display.h @@ -21,14 +21,14 @@ #include #include #include - -#include "cartographer/common/mutex.h" +#include +#include "absl/synchronization/mutex.h" #include "cartographer/common/port.h" -#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_rviz/drawable_submap.h" -#include "rviz/message_filter_display.h" -#include "rviz/properties/bool_property.h" -#include "rviz/properties/float_property.h" +#include +#include +#include #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -41,13 +41,16 @@ struct Trajectory : public QObject { Q_OBJECT public: - Trajectory(std::unique_ptr<::rviz::BoolProperty> property); + Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property, + bool pose_markers_enabled); - std::unique_ptr<::rviz::BoolProperty> visibility; + std::unique_ptr<::rviz_common::properties::BoolProperty> visibility; + std::unique_ptr<::rviz_common::properties::BoolProperty> pose_markers_visibility; std::map> submaps; private Q_SLOTS: void AllEnabledToggled(); + void PoseMarkersEnabledToggled(); }; // RViz plugin used for displaying maps which are represented by a collection of @@ -57,19 +60,22 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz::MessageFilterDisplay<::cartographer_ros_msgs::SubmapList> { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: SubmapsDisplay(); ~SubmapsDisplay() override; + + SubmapsDisplay(const SubmapsDisplay&) = delete; SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; private Q_SLOTS: void Reset(); void AllEnabledToggled(); + void PoseMarkersEnabledToggled(); void ResolutionToggled(); private: @@ -79,23 +85,27 @@ class SubmapsDisplay void onInitialize() override; void reset() override; void processMessage( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; - void update(float wall_dt, float ros_dt) override; - - ::tf2_ros::Buffer tf_buffer_; - ::tf2_ros::TransformListener tf_listener_; - ros::ServiceClient client_; - ::rviz::StringProperty* submap_query_service_property_; + const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override; + void update(float , float) override; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; + rclcpp::Client::SharedPtr client_; + ::rviz_common::properties::StringProperty* submap_query_service_property_; std::unique_ptr map_frame_; - ::rviz::StringProperty* tracking_frame_property_; + ::rviz_common::properties::StringProperty* tracking_frame_property_; Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. - std::vector> trajectories_ GUARDED_BY(mutex_); - ::cartographer::common::Mutex mutex_; - ::rviz::BoolProperty* slice_high_resolution_enabled_; - ::rviz::BoolProperty* slice_low_resolution_enabled_; - ::rviz::Property* trajectories_category_; - ::rviz::BoolProperty* visibility_all_enabled_; - ::rviz::FloatProperty* fade_out_start_distance_in_meters_; + std::map> trajectories_ GUARDED_BY(mutex_); + absl::Mutex mutex_; + ::rviz_common::properties::BoolProperty* slice_high_resolution_enabled_; + ::rviz_common::properties::BoolProperty* slice_low_resolution_enabled_; + ::rviz_common::properties::Property* trajectories_category_; + ::rviz_common::properties::BoolProperty* visibility_all_enabled_; + ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_; + ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_; + }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index a5bf7668a..5527fbc05 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -15,9 +15,9 @@ limitations under the License. --> - + cartographer_rviz - 1.0.0 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor @@ -28,30 +28,35 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors - catkin + ament_cmake - g++-static + git - cartographer - cartographer_ros - cartographer_ros_msgs - eigen_conversions + cartographer + cartographer_ros + cartographer_ros_msgs libqt5-core libqt5-gui libqt5-widgets - message_runtime + builtin_interfaces + rosidl_default_generators qtbase5-dev - roscpp - roslib - rviz + rclcpp + absl + libpcl-all-dev + pcl_conversions + rcutils + rviz2 + rviz_common - + ament_cmake + diff --git a/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/rviz_plugin_description.xml index 833b60194..fc7cb9e90 100644 --- a/cartographer_rviz/rviz_plugin_description.xml +++ b/cartographer_rviz/rviz_plugin_description.xml @@ -14,13 +14,13 @@ limitations under the License. --> - - + + base_class_type="rviz_common::Display"> Displays submaps as a unified map in RViz. - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros diff --git a/cartographer_rviz/src/.clang-format b/cartographer_rviz/src/.clang-format new file mode 100644 index 000000000..5650f22c0 --- /dev/null +++ b/cartographer_rviz/src/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +DerivePointerAlignment: false diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/src/drawable_submap.cpp similarity index 79% rename from cartographer_rviz/cartographer_rviz/drawable_submap.cc rename to cartographer_rviz/src/drawable_submap.cpp index afe9acfe4..c2b26db9e 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/src/drawable_submap.cpp @@ -23,12 +23,12 @@ #include "Eigen/Core" #include "Eigen/Geometry" -#include "cartographer/common/make_unique.h" +#include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "eigen_conversions/eigen_msg.h" -#include "ros/ros.h" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include + namespace cartographer_rviz { @@ -44,11 +44,13 @@ constexpr int kNumberOfSlicesPerSubmap = 2; } // namespace + DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, - ::rviz::DisplayContext* const display_context, + ::rviz_common::DisplayContext* const display_context, Ogre::SceneNode* const map_node, - ::rviz::Property* const submap_category, - const bool visible, const float pose_axes_length, + ::rviz_common::properties::Property* const submap_category, + const bool visible, const bool pose_axes_visible, + const float pose_axes_length, const float pose_axes_radius) : id_(id), display_context_(display_context), @@ -56,30 +58,31 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, submap_id_text_node_(submap_node_->createChildSceneNode()), pose_axes_(display_context->getSceneManager(), submap_node_, pose_axes_length, pose_axes_radius), + pose_axes_visible_(pose_axes_visible), submap_id_text_(QString("(%1,%2)") .arg(id.trajectory_id) .arg(id.submap_index) .toStdString()), - last_query_timestamp_(0) { + last_query_timestamp_(0){ for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap; ++slice_index) { - ogre_slices_.emplace_back(::cartographer::common::make_unique( + ogre_slices_.emplace_back(absl::make_unique( id, slice_index, display_context->getSceneManager(), submap_node_)); } // DrawableSubmap creates and manages its visibility property object // (a unique_ptr is needed because the Qt parent of the visibility // property is the submap_category object - the BoolProperty needs // to be destroyed along with the DrawableSubmap) - visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( + visibility_ = absl::make_unique<::rviz_common::properties::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); submap_id_text_.setCharacterHeight(kSubmapIdCharHeight); submap_id_text_.setColor(kSubmapIdColor); - submap_id_text_.setTextAlignment(::rviz::MovableText::H_CENTER, - ::rviz::MovableText::V_ABOVE); - // TODO(jihoonl): Make it toggleable. + submap_id_text_.setTextAlignment(::rviz_rendering::MovableText::H_CENTER, + ::rviz_rendering::MovableText::V_ABOVE); submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition)); submap_id_text_node_->attachObject(&submap_id_text_); + TogglePoseMarkerVisibility(); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -94,9 +97,10 @@ DrawableSubmap::~DrawableSubmap() { } void DrawableSubmap::Update( - const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata) { - ::cartographer::common::MutexLocker locker(&mutex_); + const ::std_msgs::msg::Header& header, + const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) { + (void) header; // TODO: remove unused arg ? + absl::MutexLock locker(&mutex_); metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); submap_node_->setPosition(ToOgre(pose_.translation())); @@ -112,8 +116,10 @@ void DrawableSubmap::Update( .arg(metadata_version_)); } -bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { - ::cartographer::common::MutexLocker locker(&mutex_); +bool DrawableSubmap::MaybeFetchTexture( + rclcpp::Client::SharedPtr const client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor) { + absl::MutexLock locker(&mutex_); // Received metadata version can also be lower if we restarted Cartographer. const bool newer_version_available = submap_textures_ == nullptr || @@ -128,10 +134,11 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { } query_in_progress_ = true; last_query_timestamp_ = now; - rpc_request_future_ = std::async(std::launch::async, [this, client]() { + + rpc_request_future_ = std::async(std::launch::async, [this, client, callback_group_executor]() { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client); - ::cartographer::common::MutexLocker locker(&mutex_); + ::cartographer_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000)); + absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { // We emit a signal to update in the right thread, and pass via the @@ -145,7 +152,7 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { } bool DrawableSubmap::QueryInProgress() { - ::cartographer::common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return query_in_progress_; } @@ -176,7 +183,7 @@ void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) { } void DrawableSubmap::UpdateSceneNode() { - ::cartographer::common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (size_t slice_index = 0; slice_index < ogre_slices_.size() && slice_index < submap_textures_->textures.size(); ++slice_index) { @@ -192,4 +199,9 @@ void DrawableSubmap::ToggleVisibility() { display_context_->queueRender(); } +void DrawableSubmap::TogglePoseMarkerVisibility() { + submap_id_text_node_->setVisible(pose_axes_visible_); + pose_axes_.getSceneNode()->setVisible(pose_axes_visible_); +} + } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/src/ogre_slice.cpp similarity index 90% rename from cartographer_rviz/cartographer_rviz/ogre_slice.cc rename to cartographer_rviz/src/ogre_slice.cpp index 6f1c9f111..4be255419 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/src/ogre_slice.cpp @@ -21,6 +21,9 @@ #include "OgreGpuProgramParams.h" #include "OgreImage.h" +#include "OgreMaterialManager.h" +#include "OgreTechnique.h" +#include "OgreTextureManager.h" #include "cartographer/common/port.h" namespace cartographer_rviz { @@ -34,9 +37,7 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - return std::to_string(submap_id.trajectory_id) + "-" + - std::to_string(submap_id.submap_index) + "-" + - std::to_string(slice_id); + return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id)); } } // namespace @@ -59,10 +60,9 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, slice_node_(submap_node_->createChildSceneNode()), manual_object_(scene_manager_->createManualObject( kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { - material_ = Ogre::MaterialManager::getSingleton().getByName( - kSubmapSourceMaterialName); - material_ = material_->clone(kSubmapMaterialPrefix + - GetSliceIdentifier(id_, slice_id_)); + material_ = Ogre::MaterialManager::getSingleton().getByName(kSubmapSourceMaterialName); + material_ = material_->clone( + kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_), true, "General"); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -73,9 +73,9 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, OgreSlice::~OgreSlice() { Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); - if (!texture_.isNull()) { + if (texture_) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); + texture_.reset(); } scene_manager_->destroySceneNode(slice_node_); scene_manager_->destroyManualObject(manual_object_); @@ -116,11 +116,11 @@ void OgreSlice::Update( manual_object_->end(); Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); + pixel_stream.reset(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); - if (!texture_.isNull()) { + if (texture_) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); + texture_.reset(); } const std::string texture_name = kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/src/submaps_display.cpp similarity index 50% rename from cartographer_rviz/cartographer_rviz/submaps_display.cc rename to cartographer_rviz/src/submaps_display.cpp index 8730105c2..edc8cd828 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/src/submaps_display.cpp @@ -17,19 +17,21 @@ #include "cartographer_rviz/submaps_display.h" #include "OgreResourceGroupManager.h" -#include "cartographer/common/make_unique.h" -#include "cartographer/common/mutex.h" +#include "absl/memory/memory.h" +#include "absl/synchronization/mutex.h" + #include "cartographer/mapping/id.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "geometry_msgs/TransformStamped.h" -#include "pluginlib/class_list_macros.h" -#include "ros/package.h" -#include "ros/ros.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/properties/bool_property.h" -#include "rviz/properties/string_property.h" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cartographer_rviz { @@ -44,46 +46,63 @@ constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query"; } // namespace -SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { - submap_query_service_property_ = new ::rviz::StringProperty( +SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") { + submap_query_service_property_ = new ::rviz_common::properties::StringProperty( "Submap query service", kDefaultSubmapQueryServiceName, "Submap query service to connect to.", this, SLOT(Reset())); - tracking_frame_property_ = new ::rviz::StringProperty( + tracking_frame_property_ = new ::rviz_common::properties::StringProperty( "Tracking frame", kDefaultTrackingFrame, "Tracking frame, used for fading out submaps.", this); - slice_high_resolution_enabled_ = new ::rviz::BoolProperty( + slice_high_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "High Resolution", true, "Display high resolution slices.", this, SLOT(ResolutionToggled()), this); - slice_low_resolution_enabled_ = new ::rviz::BoolProperty( + slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(""); - trajectories_category_ = new ::rviz::Property( + + callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface()); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>( + kDefaultSubmapQueryServiceName, + rmw_qos_profile_services_default, + callback_group_ + ); + trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); - visibility_all_enabled_ = new ::rviz::BoolProperty( + visibility_all_enabled_ = new ::rviz_common::properties::BoolProperty( "All", true, "Whether submaps from all trajectories should be displayed or not.", trajectories_category_, SLOT(AllEnabledToggled()), this); + pose_markers_all_enabled_ = new ::rviz_common::properties::BoolProperty( + "All Submap Pose Markers", true, + "Whether submap pose markers should be displayed or not.", + trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this); fade_out_start_distance_in_meters_ = - new ::rviz::FloatProperty("Fade-out distance", 1.f, + new ::rviz_common::properties::FloatProperty("Fade-out distance", 1.f, "Distance in meters in z-direction beyond " "which submaps will start to fade out.", this); - const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); + const std::string package_path = ament_index_cpp::get_package_share_directory("cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( - package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); + package_path + kMaterialsDirectory, "FileSystem", "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem", - ROS_PACKAGE_NAME); + "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem", - ROS_PACKAGE_NAME); + "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); } SubmapsDisplay::~SubmapsDisplay() { - client_.shutdown(); + client_.reset(); trajectories_.clear(); scene_manager_->destroySceneNode(map_node_); } @@ -91,8 +110,11 @@ SubmapsDisplay::~SubmapsDisplay() { void SubmapsDisplay::Reset() { reset(); } void SubmapsDisplay::CreateClient() { - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - submap_query_service_property_->getStdString()); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>( + submap_query_service_property_->getStdString(), + rmw_qos_profile_services_default, + callback_group_ + ); } void SubmapsDisplay::onInitialize() { @@ -103,22 +125,20 @@ void SubmapsDisplay::onInitialize() { void SubmapsDisplay::reset() { MFDClass::reset(); - ::cartographer::common::MutexLocker locker(&mutex_); - client_.shutdown(); + absl::MutexLock locker(&mutex_); + client_.reset(); trajectories_.clear(); CreateClient(); } -void SubmapsDisplay::processMessage( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - ::cartographer::common::MutexLocker locker(&mutex_); - map_frame_ = - ::cartographer::common::make_unique(msg->header.frame_id); +void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) { + absl::MutexLock locker(&mutex_); + map_frame_ = absl::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the // previous instance. - for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) { const size_t trajectory_id = submap_entry.trajectory_id; - if (trajectory_id >= trajectories_.size()) { + if (trajectories_.count(trajectory_id) == 0) { continue; } const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps; @@ -132,31 +152,40 @@ void SubmapsDisplay::processMessage( } using ::cartographer::mapping::SubmapId; std::set listed_submaps; - for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + std::set listed_trajectories; + for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) { const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index}; listed_submaps.insert(id); - while (id.trajectory_id >= static_cast(trajectories_.size())) { - trajectories_.push_back(::cartographer::common::make_unique( - ::cartographer::common::make_unique<::rviz::BoolProperty>( - QString("Trajectory %1").arg(id.trajectory_id), - visibility_all_enabled_->getBool(), - QString("List of all submaps in Trajectory %1. The checkbox " + listed_trajectories.insert(submap_entry.trajectory_id); + if (trajectories_.count(id.trajectory_id) == 0) { + trajectories_.insert(std::make_pair( + id.trajectory_id, + absl::make_unique( + absl::make_unique<::rviz_common::properties::BoolProperty>( + QString("Trajectory %1").arg(id.trajectory_id), + visibility_all_enabled_->getBool(), + QString( + "List of all submaps in Trajectory %1. The checkbox " "controls whether all submaps in this trajectory should " "be displayed or not.") - .arg(id.trajectory_id), - trajectories_category_))); + .arg(id.trajectory_id), + trajectories_category_), + pose_markers_all_enabled_->getBool()))); } auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility; auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps; + auto& pose_markers_visibility = + trajectories_[id.trajectory_id]->pose_markers_visibility; if (trajectory_submaps.count(id.submap_index) == 0) { // TODO(ojura): Add RViz properties for adjusting submap pose axes constexpr float kSubmapPoseAxesLength = 0.3f; constexpr float kSubmapPoseAxesRadius = 0.06f; trajectory_submaps.emplace( id.submap_index, - ::cartographer::common::make_unique( + absl::make_unique( id, context_, map_node_, trajectory_visibility.get(), - trajectory_visibility->getBool(), kSubmapPoseAxesLength, + trajectory_visibility->getBool(), + pose_markers_visibility->getBool(), kSubmapPoseAxesLength, kSubmapPoseAxesRadius)); trajectory_submaps.at(id.submap_index) ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool()); @@ -165,10 +194,18 @@ void SubmapsDisplay::processMessage( } trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry); } + // Remove all deleted trajectories not mentioned in the SubmapList. + for (auto it = trajectories_.begin(); it != trajectories_.end();) { + if (listed_trajectories.count(it->first) == 0) { + it = trajectories_.erase(it); + } else { + ++it; + } + } // Remove all submaps not mentioned in the SubmapList. - for (size_t trajectory_id = 0; trajectory_id < trajectories_.size(); - ++trajectory_id) { - auto& trajectory_submaps = trajectories_[trajectory_id]->submaps; + for (const auto& trajectory_by_id : trajectories_) { + const int trajectory_id = trajectory_by_id.first; + auto& trajectory_submaps = trajectory_by_id.second->submaps; for (auto it = trajectory_submaps.begin(); it != trajectory_submaps.end();) { if (listed_submaps.count( @@ -181,21 +218,21 @@ void SubmapsDisplay::processMessage( } } -void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { - ::cartographer::common::MutexLocker locker(&mutex_); +void SubmapsDisplay::update(const float , const float) { + absl::MutexLock locker(&mutex_); // Schedule fetching of new submap textures. - for (const auto& trajectory : trajectories_) { + for (const auto& trajectory_by_id : trajectories_) { int num_ongoing_requests = 0; - for (const auto& submap_entry : trajectory->submaps) { + for (const auto& submap_entry : trajectory_by_id.second->submaps) { if (submap_entry.second->QueryInProgress()) { ++num_ongoing_requests; } } - for (auto it = trajectory->submaps.rbegin(); - it != trajectory->submaps.rend() && + for (auto it = trajectory_by_id.second->submaps.rbegin(); + it != trajectory_by_id.second->submaps.rend() && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; ++it) { - if (it->second->MaybeFetchTexture(&client_)) { + if (it->second->MaybeFetchTexture(client_, callback_group_executor_)) { ++num_ongoing_requests; } } @@ -203,26 +240,27 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { if (map_frame_ == nullptr) { return; } + // Update the fading by z distance. - const ros::Time kLatest(0); + const auto klatest = this->get_clock()->now(); try { - const ::geometry_msgs::TransformStamped transform_stamped = - tf_buffer_.lookupTransform( - *map_frame_, tracking_frame_property_->getStdString(), kLatest); - for (auto& trajectory : trajectories_) { - for (auto& submap_entry : trajectory->submaps) { + const ::geometry_msgs::msg::TransformStamped transform_stamped = + tf_buffer_->lookupTransform( + *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero,std::chrono::milliseconds(100)); + for (auto& trajectory_by_id : trajectories_) { + for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetAlpha( transform_stamped.transform.translation.z, fade_out_start_distance_in_meters_->getFloat()); } } } catch (const tf2::TransformException& ex) { - ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what()); + RCLCPP_WARN(this->get_logger(), "Could not compute submap fading: %s", ex.what()); } // Update the map frame to fixed frame transform. Ogre::Vector3 position; Ogre::Quaternion orientation; - if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position, + if (context_->getFrameManager()->getTransform(*map_frame_, klatest, position, orientation)) { map_node_->setPosition(position); map_node_->setOrientation(orientation); @@ -231,17 +269,25 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } void SubmapsDisplay::AllEnabledToggled() { - ::cartographer::common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const bool visible = visibility_all_enabled_->getBool(); - for (auto& trajectory : trajectories_) { - trajectory->visibility->setBool(visible); + for (auto& trajectory_by_id : trajectories_) { + trajectory_by_id.second->visibility->setBool(visible); + } +} + +void SubmapsDisplay::PoseMarkersEnabledToggled() { + absl::MutexLock locker(&mutex_); + const bool visible = pose_markers_all_enabled_->getBool(); + for (auto& trajectory_by_id : trajectories_) { + trajectory_by_id.second->pose_markers_visibility->setBool(visible); } } void SubmapsDisplay::ResolutionToggled() { - ::cartographer::common::MutexLocker locker(&mutex_); - for (auto& trajectory : trajectories_) { - for (auto& submap_entry : trajectory->submaps) { + absl::MutexLock locker(&mutex_); + for (auto& trajectory_by_id : trajectories_) { + for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetSliceVisibility( 0, slice_high_resolution_enabled_->getBool()); submap_entry.second->SetSliceVisibility( @@ -257,12 +303,28 @@ void Trajectory::AllEnabledToggled() { } } -Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property) +void Trajectory::PoseMarkersEnabledToggled() { + const bool visible = pose_markers_visibility->getBool(); + for (auto& submap_entry : submaps) { + submap_entry.second->set_pose_markers_visibility(visible); + } +} + +Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property, + const bool pose_markers_enabled) : visibility(std::move(property)) { ::QObject::connect(visibility.get(), SIGNAL(changed()), this, SLOT(AllEnabledToggled())); + // Add toggle for submap pose markers as the first entry of the visibility + // property list of this trajectory. + pose_markers_visibility = absl::make_unique<::rviz_common::properties::BoolProperty>( + QString("Submap Pose Markers"), pose_markers_enabled, + QString("Toggles the submap pose markers of this trajectory."), + visibility.get()); + ::QObject::connect(pose_markers_visibility.get(), SIGNAL(changed()), this, + SLOT(PoseMarkersEnabledToggled())); } } // namespace cartographer_rviz -PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display) +PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, rviz_common::Display) diff --git a/docs/source/algo_walkthrough.rst b/docs/source/algo_walkthrough.rst new file mode 100644 index 000000000..42b056a57 --- /dev/null +++ b/docs/source/algo_walkthrough.rst @@ -0,0 +1,366 @@ +.. Copyright 2018 The Cartographer Authors + +.. 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. + +Algorithm walkthrough for tuning +================================ + +Cartographer is a complex system and tuning it requires a good understanding of its inner working. +This page tries to give an intuitive overview of the different subsystems used by Cartographer along with their configuration values. +If you are interested in more than an introduction to Cartographer, you should refer to the Cartographer paper. +It only describes the 2D SLAM but it defines rigourously most of the concepts described here. +Those concepts generally apply to 3D as well. + +W. Hess, D. Kohler, H. Rapp, and D. Andor, +`Real-Time Loop Closure in 2D LIDAR SLAM`_, in +*Robotics and Automation (ICRA), 2016 IEEE International Conference on*. +IEEE, 2016. pp. 1271–1278. + +.. _Real-Time Loop Closure in 2D LIDAR SLAM: https://research.google.com/pubs/pub45466.html + +Overview +-------- + +.. image:: https://raw.githubusercontent.com/cartographer-project/cartographer/master/docs/source/high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png + +Cartographer can be seen as two separate, but related subsystems. +The first one is **local SLAM** (sometimes also called **frontend** or local trajectory builder). +Its job is to build a succession of **submaps**. +Each submap is meant to be locally consistent but we accept that local SLAM drifts over time. +Most of the local SLAM options can be found in `install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua`_ for 2D and `install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua`_ for 3D. (for the rest of this page we will refer to `TRAJECTORY_BUILDER_nD` for the common options) + +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_2d.lua +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_3d.lua + +The other subsystem is **global SLAM** (sometimes called the **backend**). +It runs in background threads and its main job is to find **loop closure constraints**. +It does that by scan-matching **scans** (gathered in **nodes**) against submaps. +It also incorporates other sensor data to get a higher level view and identify the most consistent global solution. +In 3D, it also tries to find the direction of gravity. +Most of its options can be found in `install_isolated/share/cartographer/configuration_files/pose_graph.lua`_ + +.. _install_isolated/share/cartographer/configuration_files/pose_graph.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/pose_graph.lua + +On a higher abstraction, the job of local SLAM is to generate good submaps and the job of global SLAM is to tie them most consistently together. + +Input +----- + +Range finding sensors (for example: LIDARs) provide depth information in multiple directions. +However, some of the measurements are irrelevant for SLAM. +If the sensor is partially covered with dust or if it is directed towards a part of the robot, some of the measured distance can be considered as noise for SLAM. +On the other hand, some of the furthest measurements can also come from undesired sources (reflection, sensor noise) and are irrelevant for SLAM as well. +To tackle those issue, Cartographer starts by applying a bandpass filter and only keeps range values between a certain min and max range. +Those min and max values should be chosen according to the specifications of your robot and sensors. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.min_range + TRAJECTORY_BUILDER_nD.max_range + +.. note:: + + In 2D, Cartographer replaces ranges further than max_range by ``TRAJECTORY_BUILDER_2D.missing_data_ray_length``. It also provides a ``max_z`` and ``min_z`` values to filter 3D point clouds into a 2D cut. + +.. note:: + + In Cartographer configuration files, every distance is defined in meters + +Distances are measured over a certain period of time, while the robot is actually moving. +However, distances are delivered by sensors "in batch" in large ROS messages. +Each of the messages' timestamp can be considered independently by Cartographer to take into account deformations caused by the robot's motion. +The more often Cartographer gets measurements, the better it becomes at unwarping the measurements to assemble a single coherent scan that could have been captured instantly. +It is therefore strongly encouraged to provide as many range data (ROS messages) by scan (a set of range data that can be matched against another scan) as possible. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.num_accumulated_range_data + +Range data is typically measured from a single point on the robot but in multiple angles. +This means that close surfaces (for instance the road) are very often hit and provide lots of points. +On the opposite, far objects are less often hit and offer less points. +In order to reduce the computational weight of points handling, we usually need to subsample point clouds. +However, a simple random sampling would remove points from areas where we already have a low density of measurements and the high-density areas would still have more points than needed. +To address that density problem, we can use a voxel filter that downsamples raw points into cubes of a constant size and only keeps the centroid of each cube. + +A small cube size will result in a more dense data representation, causing more computations. +A large cube size will result in a data loss but will be much quicker. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.voxel_filter_size + +After having applied a fixed-size voxel filter, Cartographer also applies an **adaptive voxel filter**. +This filter tries to determine the optimal voxel size (under a max length) to achieve a target number of points. +In 3D, two adaptive voxel filters are used to generate a high resolution and a low resolution point clouds, their usage will be clarified in :ref:`local-slam`. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length + TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points + +An Inertial Measurement Unit can be an useful source of information for SLAM because it provides an accurate direction of gravity (hence, of the ground) and a noisy but good overall indication of the robot's rotation. +In order to filter the IMU noise, gravity is observed over a certain amount of time. +If you use 2D SLAM, range data can be handled in real-time without an additional source of information so you can choose whether you'd like Cartographer to use an IMU or not. +With 3D SLAM, you need to provide an IMU because it is used as an initial guess for the orientation of the scans, greatly reducing the complexity of scan matching. + + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.use_imu_data + TRAJECTORY_BUILDER_nD.imu_gravity_time_constant + +.. note:: + + In Cartographer configuration files, every time value is defined in seconds + +.. _local-slam: + +Local SLAM +---------- + +Once a scan has been assembled and filtered from multiple range data, it is ready for the local SLAM algorithm. +Local SLAM inserts a new scan into its current submap construction by **scan matching** using an initial guess from the **pose extrapolator**. +The idea behind the pose extrapolator is to use sensor data of other sensors besides the range finder to predict where the next scan should be inserted into the submap. + +Two scan matching strategies are available: + +- The ``CeresScanMatcher`` takes the initial guess as prior and finds the best spot where the scan match fits the submap. + It does this by interpolating the submap and sub-pixel aligning the scan. + This is fast, but cannot fix errors that are significantly larger than the resolution of the submaps. + If your sensor setup and timing is reasonable, using only the ``CeresScanMatcher`` is usually the best choice to make. +- The ``RealTimeCorrelativeScanMatcher`` can be enabled if you do not have other sensors or you do not trust them. + It uses an approach similar to how scans are matched against submaps in loop closure (described later), but instead it matches against the current submap. + The best match is then used as prior for the ``CeresScanMatcher``. + This scan matcher is very expensive and will essentially override any signal from other sensors but the range finder, but it is robust in feature rich environments. + +Either way, the ``CeresScanMatcher`` can be configured to give a certain weight to each of its input. +The weight is a measure of trust into your data, this can be seen as a static covariance. +The unit of weight parameters are dimensionless quantities and can't be compared between each others. +The bigger the weight of a source of data is, the more emphasis Cartographer will put on this source of data when doing scan matching. +Sources of data include occupied space (points from the scan), translation and rotation from the pose extrapolator (or ``RealTimeCorrelativeScanMatcher``) + +.. code-block:: lua + + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0 + TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1 + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight + +.. note:: + + In 3D, the ``occupied_space_weight_0`` and ``occupied_space_weight_1`` parameters are related, respectively, to the high resolution and low resolution filtered point clouds. + +The ``CeresScanMatcher`` gets its name from `Ceres Solver`_, a library developed at Google to solve non-linear least squares problems. +The scan matching problem is modelled as the minimization of such a problem with the **motion** (a transformation matrix) between two scans being a parameter to determine. +Ceres optimizes the motion using a descent algorithm for a given number of iterations. +Ceres can be configured to adapt the convergence speed to your own needs. + +.. _Ceres Solver: http://ceres-solver.org/ + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.max_num_iterations + TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.num_threads + +The ``RealTimeCorrelativeScanMatcher`` can be toggled depending on the trust you have in your sensors. +It works by searching for similar scans in a **search window** which is defined by a maximum distance radius and a maximum angle radius. +When performing scan matching with scans found in this window, a different weight can be chosen for the translational and rotational components. +You can play with those weight if, for example, you know that your robot doesn't rotate a lot. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.use_online_correlative_scan_matching + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.linear_search_window + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.angular_search_window + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.translation_delta_cost_weight + TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.rotation_delta_cost_weight + +To avoid inserting too many scans per submaps, once a motion between two scans is found by the scan matcher, it goes through a **motion filter**. +A scan is dropped if the motion that led to it is not considered as significant enough. +A scan is inserted into the current submap only if its motion is above a certain distance, angle or time threshold. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds + TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters + TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians + +A submap is considered as complete when the local SLAM has received a given amount of range data. +Local SLAM drifts over time, global SLAM is used to fix this drift. +Submaps must be small enough so that the drift inside them is below the resolution, so that they are locally correct. +On the other hand, they should be large enough to be distinct for loop closure to work properly. + +.. code-block:: lua + + TRAJECTORY_BUILDER_nD.submaps.num_range_data + +Submaps can store their range data in a couple of different data structures: +The most widely used representation is called probability grids. +However, in 2D, one can also choose to use Truncated Signed Distance Fields (TSDF). + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type + +Probability grids cut out space into a 2D or 3D table where each cell has a fixed size and contains the odds of being obstructed. +Odds are updated according to "*hits*" (where the range data is measured) and "*misses*" (the free space between the sensor and the measured points). +Both *hits* and *misses* can have a different weight in occupancy probability calculations giving more or less trust to occupied or free space measurements. + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability + TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability + TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit_probability + TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.miss_probability + +In 2D, only one probability grid per submap is stored. +In 3D, for scan matching performance reasons, two *hybrid* probability grids are used. +(the term "hybrid" only refers to an internal tree-like data representation and is abstracted to the user) + +- a low resolution hybrid grid for far measurements +- a high resolution hybrid grid for close measurements + +Scan matching starts by aligning far points of the low resolution point cloud with the low resolution hybrid grid and then refines the pose by aligning the close high resolution points with the high resolution hybrid grid. + +.. code-block:: lua + + TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution + TRAJECTORY_BUILDER_3D.submaps.high_resolution + TRAJECTORY_BUILDER_3D.submaps.low_resolution + TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range + TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range + +.. note:: + + Cartographer ROS provides an RViz plugin to visualize submaps. You can select the submaps you want to see from their number. In 3D, RViz only shows 2D projections of the 3D hybrid probability grids (in grayscale). Options are made available in RViz's left pane to switch between the low and high resolution hybrid grids visualization. + +**TODO**: *Documenting TSDF configuration* + +Global SLAM +----------- + +While the local SLAM generates its succession of submaps, a global optimization (usually refered to as "*the optimization problem*" or "*sparse pose adjustment*") task runs in background. +Its role is to re-arrange submaps between each other so that they form a coherent global map. +For instance, this optimization is in charge of altering the currently built trajectory to properly align submaps with regards to loop closures. + +The optimization is run in batches once a certain number of trajectory nodes was inserted. Depending on how frequently you need to run it, you can tune the size of these batches. + +.. code-block:: lua + + POSE_GRAPH.optimize_every_n_nodes + +.. note:: + + Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way to disable global SLAM and concentrate on the behavior of local SLAM. This is usually one of the first thing to do to tune Cartographer. + +The global SLAM is a kind of "*GraphSLAM*", it is essentially a pose graph optimization which works by building **constraints** between **nodes** and submaps and then optimizing the resulting constraints graph. +Constraints can intuitively be thought of as little ropes tying all nodes together. +The sparse pose adjustement fastens those ropes altogether. +The resulting net is called the "*pose graph*". + +.. note:: + + Constraints can be visualized in RViz, it is very handy to tune global SLAM. One can also toggle ``POSE_GRAPH.constraint_builder.log_matches`` to get regular reports of the constraints builder formatted as histograms. + +- Non-global constraints (also known as intra submaps constraints) are built automatically between nodes that are closely following each other on a trajectory. + Intuitively, those "*non-global ropes*" keep the local structure of the trajectory coherent. +- Global constraints (also referred to as loop closure constraints or inter submaps contraints) are regularly searched between a new submap and previous nodes that are considered "*close enough*" in space (part of a certain **search window**) and a strong fit (a good match when running scan matching). + Intuitively, those "*global ropes*" introduce knots in the structure and firmly bring two strands closer. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.max_constraint_distance + POSE_GRAPH.fast_correlative_scan_matcher.linear_search_window + POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_xy_search_window + POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_z_search_window + POSE_GRAPH.fast_correlative_scan_matcher*.angular_search_window + +.. note:: + + In practice, global constraints can do more than finding loop closures on a single trajectory. They can also align different trajectories recorded by multiple robots but we will keep this usage and the parameters related to "global localization" out of the scope of this document. + +To limit the amount of constraints (and computations), Cartographer only considers a subsampled set of all close nodes for constraints building. +This is controlled by a sampling ratio constant. +Sampling too few nodes could result in missed constraints and ineffective loop closures. +Sampling too many nodes would slow the global SLAM down and prevent real-time loop closures. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.sampling_ratio + +When a node and a submap are considered for constraint building, they go through a first scan matcher called the ``FastCorrelativeScanMatcher``. +This scan matcher has been specifically designed for Cartographer and makes real-time loop closures scan matching possible. +The ``FastCorrelativeScanMatcher`` relies on a "*Branch and bound*" mechanism to work at different grid resolutions and efficiently eliminate incorrect matchings. +This mechanism is extensively presented in the Cartographer paper presented earlier in this document. +It works on an exploration tree whose depth can be controlled. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth + POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth + +Once the ``FastCorrelativeScanMatcher`` has a good enough proposal (above a minimum score of matching), it is then fed into a Ceres Scan Matcher to refine the pose. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.min_score + POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d + POSE_GRAPH.constraint_builder.ceres_scan_matcher + +When Cartographer runs *the optimization problem*, Ceres is used to rearrange submaps according to multiple *residuals*. +Residuals are calculated using weighted cost functions. +The global optimization has cost functions to take into account plenty of data sources: the global (loop closure) constraints, the non-global (matcher) constraints, the IMU acceleration and rotation measurements, the local SLAM rough pose estimations, an odometry source or a fixed frame (such as a GPS system). +The weights and Ceres options can be configured as described in the :ref:`local-slam` section. + +.. code-block:: lua + + POSE_GRAPH.constraint_builder.loop_closure_translation_weight + POSE_GRAPH.constraint_builder.loop_closure_rotation_weight + POSE_GRAPH.matcher_translation_weight + POSE_GRAPH.matcher_rotation_weight + POSE_GRAPH.optimization_problem.*_weight + POSE_GRAPH.optimization_problem.ceres_solver_options + +.. note:: + + One can find useful information about the residuals used in the optimization problem by toggling ``POSE_GRAPH.max_num_final_iterations`` + +As part of its IMU residual, the optimization problem gives some flexibility to the IMU pose and, by default, Ceres is free to optimize the extrinsic calibration between your IMU and tracking frame. +If you don't trust your IMU pose, the results of Ceres' global optimization can be logged and used to improve your extrinsic calibration. +If Ceres doesn't optimize your IMU pose correctly and you trust your extrinsic calibration enough, you can make this pose constant. + +.. code-block:: lua + + POSE_GRAPH.optimization_problem.log_solver_summary + POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d + +In residuals, the influence of outliers is handled by a **Huber loss** function configured with a certain a Huber scale. +The bigger the Huber scale, `the higher is the impact`_ of (potential) outliers. + +.. _the higher is the impact: https://github.com/ceres-solver/ceres-solver/blob/0d3a84fce553c9f7aab331f0895fa7b1856ef5ee/include/ceres/loss_function.h#L172 + +.. code-block:: lua + + POSE_GRAPH.optimization_problem.huber_scale + +Once the trajectory is finished, Cartographer runs a new global optimization with, typically, a lot more iterations than previous global optimizations. +This is done to polish the final result of Cartographer and usually does not need to be real-time so a large number of iterations is often a right choice. + +.. code-block:: lua + + POSE_GRAPH.max_num_final_iterations diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index 21967ff44..c0315fe42 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -17,42 +17,43 @@ .. _assets_writer: -Assets writer -============= +Exploiting the map generated by Cartographer ROS +================================================ -The purpose of SLAM is to compute the trajectory of a single sensor through a metric space. -On a higher level, the input of SLAM is sensor data, its output is the best estimate of the trajectory up to this point in time. -To be real-time and efficient, Cartographer throws most of the sensor data away immediately. +As sensor data come in, the state of a SLAM algorithm such as Cartographer evolves to stay *the current best estimate* of a robot's trajectory and surroundings. +The most accurate localization and mapping Cartographer can offer is therefore the one obtained when the algorithm finishes. +Cartographer can serialize its internal state in a ``.pbstream`` file format which is essentially a compressed protobuf file containing a snapshot of the data structures used by Cartographer internally. -The trajectory alone is rarely of interest. -But once the best trajectory is established, the full sensor data can be used to derive and visualize information about its surroundings. +To run efficiently in real-time, Cartographer throws most of its sensor data away immediately and only works with a small subset of its input, the mapping used internally (and saved in ``.pbstream`` files) is then very rough. +However, when the algorithm finishes and a best trajectory is established, it can be recombined *a posteriori* with the full sensors data to create a high resolution map. -Cartographer provides the assets writer for this. -Its inputs are +Cartographer makes this kind of recombination possible using ``cartographer_assets_writer``. +The assets writer takes as input -1. the original sensor data fed to SLAM in a ROS bag file, -2. the cartographer state, which is contained in the ``.pbstream`` file that SLAM creates, -3. the sensor extrinsics (i.e. TF data from the bag or a URDF), +1. the original sensors data that has been used to perform SLAM (in a ROS ``.bag`` file), +2. a cartographer state captured while performing SLAM on this sensor data (saved in a ``.pbstream`` file), +3. the sensor extrinsics (i.e. TF data from the bag or an URDF description), 4. and a pipeline configuration, which is defined in a ``.lua`` file. -The assets writer runs through the sensor data in batches with a known trajectory. -It can be used to color, filter and export SLAM point cloud data in a variety of formats. -For more information on what the asset writer can be used for, refer to the examples below below and the header files in `cartographer/io`_. +The assets writer runs through the ``.bag`` data in batches with the trajectory found in the ``.pbstream``. +The pipeline can be used to color, filter and export SLAM point cloud data into a variety of formats. +There are multiple of such points processing steps that can be interleaved in a pipeline - several ones are already available from `cartographer/io`_. -Sample usage +Sample Usage ------------ +When running Cartographer with an offline node, a ``.pbstream`` file is automatically saved. +For instance, with the 3D backpack example: + .. code-block:: bash - # Download the 3D backpack example bag. wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-14-14-00.bag - - # Launch the 3D offline demo. roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag -Watch the output on the commandline until the offline node terminates. +Watch the output on the commandline until the node terminates. It will have written ``b3-2016-04-05-14-14-00.bag.pbstream`` which represents the Cartographer state after it processed all data and finished all optimizations. -You could have gotten the same state data by running the online node and calling: + +When running as an online node, Cartographer doesn't know when your bag (or sensor input) ends so you need to use the exposed services to explicitly finish the current trajectory and make Cartographer serialize its current state: .. code-block:: bash @@ -60,11 +61,12 @@ You could have gotten the same state data by running the online node and calling rosservice call /finish_trajectory 0 # Ask Cartographer to serialize its current state. - rosservice call /write_state ${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream + # (press tab to quickly expand the parameter syntax) + rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: "true"}" -Now we run the assets writer with the `sample configuration file`_ for the 3D backpack: +Once you've retrieved your ``.pbstream`` file, you can run the assets writer with the `sample pipeline`_ for the 3D backpack: -.. _sample configuration file: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua +.. _sample pipeline: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua .. code-block:: bash @@ -72,43 +74,60 @@ Now we run the assets writer with the `sample configuration file`_ for the 3D ba bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag \ pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream -All output files are prefixed by ``--output_file_prefix`` which defaults to the filename of the first bag. +All output files are prefixed with ``--output_file_prefix`` which defaults to the filename of the first bag. For the last example, if you specify ``points.ply`` in the pipeline configuration file, this will translate to ``${HOME}/Downloads/b3-2016-04-05-14-14-00.bag_points.ply``. Configuration ------------- -The assets writer is modeled as a pipeline. -It consists of `PointsProcessor`_\ s and `PointsBatch`_\ s flow through it. -Data flows from the first processor to the next, each has the chance to modify the ``PointsBatch`` before passing it on. +The assets writer is modeled as a pipeline of `PointsProcessor`_ steps. +`PointsBatch`_ data flows through each processor and they all have the chance to modify the ``PointsBatch`` before passing it on. -.. _PointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h -.. _PointsBatch: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h +.. _PointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h +.. _PointsBatch: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h -For example the `assets_writer_backpack_3d.lua`_ uses ``min_max_range_filter`` to remove points that are either too close or too far from the sensor. -After this, it writes X-Rays, then recolors the ``PointsBatch``\ s depending on the sensor frame ids and writes another set of X-Rays using these new colors. +For example the `assets_writer_backpack_3d.lua`_ pipeline uses ``min_max_range_filter`` to remove points that are either too close or too far from the sensor. +After this, it saves "*X-Rays*" (translucent side views of the map), then recolors the ``PointsBatch``\ s depending on the sensor frame ids and writes another set of X-Rays using these new colors. -.. _assets_writer_backpack_3d.lua: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua +.. _assets_writer_backpack_3d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua -The individual ``PointsProcessor``\ s are all in the `cartographer/io`_ sub-directory and documented in their individual header files. +The available ``PointsProcessor``\ s are all defined in the `cartographer/io`_ sub-directory and documented in their individual header files. -.. _cartographer/io: https://github.com/googlecartographer/cartographer/tree/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io +.. _cartographer/io: https://github.com/cartographer-project/cartographer/tree/f1ac8967297965b8eb6f2f4b08a538e052b5a75b/cartographer/io + +* **color_points**: Colors points with a fixed color by frame_id. +* **dump_num_points**: Passes through points, but keeps track of how many points it saw and output that on Flush. +* **fixed_ratio_sampler**: Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1. makes this filter a no-op. +* **frame_id_filter**: Filters all points with blacklisted frame_id or a non-whitelisted frame id. Note that you can either specify the whitelist or the blacklist, but not both at the same time. +* **write_hybrid_grid**: Creates a hybrid grid of the points with voxels being 'voxel_size' big. 'range_data_inserter' options are used to configure the range data ray tracing through the hybrid grid. +* **intensity_to_color**: Applies ('intensity' - min) / (max - min) * 255 and color the point grey with this value for each point that comes from the sensor with 'frame_id'. If 'frame_id' is empty, this applies to all points. +* **min_max_range_filtering**: Filters all points that are farther away from their 'origin' as 'max_range' or closer than 'min_range'. +* **voxel_filter_and_remove_moving_objects**: Voxel filters the data and only passes on points that we believe are on non-moving objects. +* **write_pcd**: Streams a PCD file to disk. The header is written in 'Flush'. +* **write_ply**: Streams a PLY file to disk. The header is written in 'Flush'. +* **write_probability_grid**: Creates a probability grid with the specified 'resolution'. As all points are projected into the x-y plane the z component of the data is ignored. 'range_data_inserter' options are used to configure the range data ray tracing through the probability grid. +* **write_xray_image**: Creates X-ray cuts through the points with pixels being 'voxel_size' big. +* **write_xyz**: Writes ASCII xyz points. First-person visualization of point clouds ------------------------------------------ -Generating a fly through of points is a two step approach: -First, write a PLY file with the points you want to visualize, then use `point_cloud_viewer`_. +Two ``PointsProcessor``\ s are of particular interest: ``pcd_writing`` and ``ply_writing`` can save a point cloud in a ``.pcd`` or ``.ply`` file format. +These file formats can then be used by specialized software such as `point_cloud_viewer`_ or `meshlab`_ to navigate through the high resolution map. + +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer +.. _meshlab: http://www.meshlab.net/ -.. _point_cloud_viewer: https://github.com/googlecartographer/point_cloud_viewer +The typical assets writer pipeline for this outcome is composed of an IntensityToColorPointsProcessor_ giving points a non-white color, then a PlyWritingPointsProcessor_ exporting the results to a ``.ply`` point cloud. +An example of such a pipeline is in `assets_writer_backpack_2d.lua`_. -The first step is usually accomplished by using IntensityToColorPointsProcessor_ to give the points a non-white color, then writing them to a PLY using PlyWritingPointsProcessor_. -An example is in `assets_writer_backpack_2d.lua`_. +.. _IntensityToColorPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/intensity_to_color_points_processor.cc +.. _PlyWritingPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/ply_writing_points_processor.h +.. _assets_writer_backpack_2d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua -.. _IntensityToColorPointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/intensity_to_color_points_processor.cc -.. _PlyWritingPointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/ply_writing_points_processor.h -.. _assets_writer_backpack_2d.lua: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua +Once you have the ``.ply``, follow the README of `point_cloud_viewer`_ to generate an on-disk octree data structure which can be viewed by one of the viewers (SDL or web based) in the same repo. +Note that color is required for ``point_cloud_viewer`` to function. -Once you have the PLY, follow the README of `point_cloud_viewer`_ to generate an on-disk octree data structure which can be viewed by one of the viewers in the same repo. +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer -.. _point_cloud_viewer: https://github.com/googlecartographer/point_cloud_viewer +.. image:: point_cloud_viewer_demo_3d.jpg diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst new file mode 100644 index 000000000..ace433fd5 --- /dev/null +++ b/docs/source/compilation.rst @@ -0,0 +1,97 @@ +.. Copyright 2018 The Cartographer Authors + +.. 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. + +========================== +Compiling Cartographer ROS +========================== + +System Requirements +=================== + +The Cartographer ROS requirements are the same as `the ones from Cartographer`_. + +The following `ROS distributions`_ are currently supported: + +* Kinetic +* Melodic +* Noetic + +.. _the ones from Cartographer: https://google-cartographer.readthedocs.io/en/latest/#system-requirements +.. _ROS distributions: http://wiki.ros.org/Distributions + +Building & Installation +======================= + +In order to build Cartographer ROS, we recommend using `wstool `_ and `rosdep +`_. For faster builds, we also recommend using +`Ninja `_. + +On Ubuntu Focal with ROS Noetic use these commands to install the above tools: + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow + +On older distributions: + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y python-wstool python-rosdep ninja-build stow + +After the tools are installed, create a new cartographer_ros workspace in 'catkin_ws'. + +.. code-block:: bash + + mkdir catkin_ws + cd catkin_ws + wstool init src + wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall + wstool update -t src + +Now you need to install cartographer_ros' dependencies. +First, we use ``rosdep`` to install the required packages. +The command 'sudo rosdep init' will print an error if you have already executed it since installing ROS. This error can be ignored. + +.. code-block:: bash + + sudo rosdep init + rosdep update + rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y + + # Only on Ubuntu 16 / ROS Kinetic: src/cartographer/scripts/install_proto3.sh + +Cartographer uses the `abseil-cpp`_ library that needs to be manually installed using this script: + +.. code-block:: bash + + src/cartographer/scripts/install_abseil.sh + +Due to conflicting versions you might need to uninstall the ROS abseil-cpp using + +.. code-block:: bash + + sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp + +Build and install. + +.. code-block:: bash + + catkin_make_isolated --install --use-ninja + +This builds Cartographer from the latest HEAD of the master branch. +If you want a specific version, you need to change the version in the cartographer_ros.rosinstall. + +.. _abseil-cpp: https://abseil.io/ diff --git a/docs/source/conf.py b/docs/source/conf.py index ebbe1fbb0..fc8fe8d28 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -28,6 +28,7 @@ import sys import os +from datetime import datetime # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the @@ -62,16 +63,16 @@ # General information about the project. project = u'Cartographer ROS' -copyright = u'2016 The Cartographer Authors' +copyright = u'{year} The Cartographer Authors'.format(year=datetime.now().year) # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. -version = '1.0.0' +#version = '' # The full version, including alpha/beta/rc tags. -release = '1.0.0' +#release = '' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -144,7 +145,7 @@ # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = [] # Add any extra paths that contain custom files (such as robots.txt or # .htaccess) here, relative to this directory. These files are copied diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index d473de40b..67a59e0a4 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -12,9 +12,9 @@ See the License for the specific language governing permissions and limitations under the License. -============= -Configuration -============= +========================================= +Lua configuration reference documentation +========================================= Note that Cartographer's ROS integration uses `tf2`_, thus all frame IDs are expected to contain only a frame name (lower-case with underscores) and no @@ -51,10 +51,26 @@ provide_odom_frame If enabled, the local, non-loop-closed, continuous pose will be published as the *odom_frame* in the *map_frame*. +publish_frame_projected_to_2d + If enabled, the published pose will be restricted to a pure 2D pose (no roll, + pitch, or z-offset). This prevents potentially unwanted out-of-plane poses in + 2D mode that can occur due to the pose extrapolation step (e.g. if the pose + shall be published as a 'base-footprint'-like frame) + use_odometry If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry must be provided in this case, and the information will be included in SLAM. +use_nav_sat + If enabled, subscribes to `sensor_msgs/NavSatFix`_ on the topic "fix". + Navigation data must be provided in this case, and the information will be + included in the global SLAM. + +use_landmarks + If enabled, subscribes to `cartographer_ros_msgs/LandmarkList`_ on the topic + "landmarks". Landmarks must be provided, as `cartographer_ros_msgs/LandmarkEntry`_ within `cartographer_ros_msgs/LandmarkList`_. If `cartographer_ros_msgs/LandmarkEntry`_ data is provided the information + will be included in the SLAM accoding to the ID of the `cartographer_ros_msgs/LandmarkEntry`_. The `cartographer_ros_msgs/LandmarkList`_ should be provided at a sample rate comparable to the other sensors. The list can be empty but has to be provided because Cartographer strictly time orders sensor data in order to make the landmarks deterministic. However it is possible to set the trajectory builder option "collate_landmarks" to false and allow for a non-deterministic but also non-blocking approach. + num_laser_scans Number of laser scan topics to subscribe to. Subscribes to `sensor_msgs/LaserScan`_ on the "scan" topic for one laser scanner, or topics @@ -87,15 +103,40 @@ pose_publish_period_sec Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of 200 Hz. +publish_to_tf + Enable or disable providing of TF transforms. + +publish_tracked_pose + Enable publishing of tracked pose as a `geometry_msgs/PoseStamped`_ to topic "tracked_pose". + trajectory_publish_period_sec Interval in seconds at which to publish the trajectory markers, e.g. 30e-3 for 30 milliseconds. +rangefinder_sampling_ratio + Fixed ratio sampling for range finders messages. + +odometry_sampling_ratio + Fixed ratio sampling for odometry messages. + +fixed_frame_sampling_ratio + Fixed ratio sampling for fixed frame messages. + +imu_sampling_ratio + Fixed ratio sampling for IMU messages. + +landmarks_sampling_ratio + Fixed ratio sampling for landmarks messages. + .. _REP 105: http://www.ros.org/reps/rep-0105.html .. _ROS Names: http://wiki.ros.org/Names +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _sensor_msgs/LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html .. _sensor_msgs/MultiEchoLaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html .. _sensor_msgs/PointCloud2: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html +.. _sensor_msgs/NavSatFix: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html +.. _cartographer_ros_msgs/LandmarkList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/LandmarkList.msg +.. _cartographer_ros_msgs/LandmarkEntry: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkEntry.msg .. _tf2: http://wiki.ros.org/tf2 diff --git a/docs/source/data.rst b/docs/source/data.rst index ba29eaa4c..f92853d51 100644 --- a/docs/source/data.rst +++ b/docs/source/data.rst @@ -164,6 +164,39 @@ Data `b3-2016-06-07-12-42-49.bag `_ 596 s 3.9 GB 3 gaps in horizontal laser data, no intensities ==================================================================================================================================================== ======== ====== ================================================ +MiR +=========================================== + +This data was collected using `MiR100 `_. +An additional Logitech Webcam C930e Full HD camera was attached on top to +collect images for landmark detection. + +License +------- + +Copyright 2018 The Cartographer Authors + +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. + +Data +---- + +==================================================================================================================================================== ======== ======= +`ROS Bag `_ Duration Size +==================================================================================================================================================== ======== ======= +`landmarks_demo_uncalibrated.bag `_ 180 s 41.7 MB +==================================================================================================================================================== ======== ======= + PR2 – Willow Garage =================== diff --git a/docs/source/demo_2d.gif b/docs/source/demo_2d.gif new file mode 100644 index 000000000..f6df65adb Binary files /dev/null and b/docs/source/demo_2d.gif differ diff --git a/docs/source/demos.rst b/docs/source/demos.rst index 99b85967f..dac214f36 100644 --- a/docs/source/demos.rst +++ b/docs/source/demos.rst @@ -12,68 +12,115 @@ See the License for the specific language governing permissions and limitations under the License. -===== -Demos -===== +====================================== +Running Cartographer ROS on a demo bag +====================================== + +Now that Cartographer and Cartographer's ROS integration are installed, you can +download example bags (e.g. 2D and 3D backpack collections of the +`Deutsches Museum `_) to a +known location, in this case ``~/Downloads``, and use ``roslaunch`` to bring up +the demo. + +The launch files will bring up ``roscore`` and ``rviz`` automatically. + +.. warning:: When you want to run cartographer_ros, you might need to source your ROS environment by running ``source install_isolated/setup.bash`` first (replace bash with zsh if your shell is zsh) + +Deutsches Museum +================ + +Download and launch the 2D backpack demo: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag + roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag + +Download and launch the 3D backpack demo: + +.. code-block:: bash + + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag + roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag Pure localization ================= - .. code-block:: bash +Pure localization uses 2 different bags. The first one is used to generate the map, the second to run pure localization. + +Download the 2D bags from the Deutsche Museum: + +.. code-block:: bash - # Pure localization demo in 2D: We use 2 different 2D bags from the Deutsche - # Museum. The first one is used to generate the map, the second to run - # pure localization. wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag - # Generate the map: Run the next command, wait until cartographer_offline_node finishes. + +Generate the map (wait until cartographer_offline_node finishes) and then run pure localization: + +.. code-block:: bash + roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag - # Run pure localization: roslaunch cartographer_ros demo_backpack_2d_localization.launch \ load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \ bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag - # Pure localization demo in 3D: We use 2 different 3D bags from the Deutsche - # Museum. The first one is used to generate the map, the second to run - # pure localization. +Download the 3D bags from the Deutsche Museum: + +.. code-block:: bash + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag - # Generate the map: Run the next command, wait until cartographer_offline_node finishes. + +Generate the map (wait until cartographer_offline_node finishes) and then run pure localization: + +.. code-block:: bash + roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag - # Run pure localization: roslaunch cartographer_ros demo_backpack_3d_localization.launch \ load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \ bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag +Static landmarks +================ + + .. raw:: html + + + + .. code-block:: bash + + # Download the landmarks example bag. + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/mir/landmarks_demo_uncalibrated.bag + + # Launch the landmarks demo. + roslaunch cartographer_mir offline_mir_100_rviz.launch bag_filename:=${HOME}/Downloads/landmarks_demo_uncalibrated.bag + Revo LDS ======== - .. code-block:: bash +Download and launch an example bag captured from a low-cost Revo Laser Distance Sensor from Neato Robotics vacuum cleaners: - # Download the Revo LDS example bag. - wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag +.. code-block:: bash - # Launch the Revo LDS demo. + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag roslaunch cartographer_ros demo_revo_lds.launch bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds.bag PR2 === - .. code-block:: bash +Download and launch an example bag captured from a PR2 R&D humanoid robot from Willow Garage: - # Download the PR2 example bag. - wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bag +.. code-block:: bash - # Launch the PR2 demo. + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bag roslaunch cartographer_ros demo_pr2.launch bag_filename:=${HOME}/Downloads/2011-09-15-08-32-46.bag Taurob Tracker ============== - .. code-block:: bash +Download and launch an example bag captured from a Taurob Tracker teleoperation robot: - # Download the Taurob Tracker example bag. - wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/taurob_tracker/taurob_tracker_simulation.bag +.. code-block:: bash - # Launch the Taurob Tracker demo. + wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/taurob_tracker/taurob_tracker_simulation.bag roslaunch cartographer_ros demo_taurob_tracker.launch bag_filename:=${HOME}/Downloads/taurob_tracker_simulation.bag diff --git a/docs/source/faq.rst b/docs/source/faq.rst index ce72a7ae1..c0e607fb8 100644 --- a/docs/source/faq.rst +++ b/docs/source/faq.rst @@ -33,7 +33,7 @@ incorporates motion estimation by combining constant velocity and IMU measurements, for matching. Since there are two VLP-16s, 160 UDP packets is enough for roughly 2 revolutions, one per VLP-16. -__ https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/configuration_files/backpack_3d.lua +__ https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/configuration_files/backpack_3d.lua Why is IMU data required for 3D SLAM but not for 2D? ---------------------------------------------------- diff --git a/docs/source/frames_demo_2d.jpg b/docs/source/frames_demo_2d.jpg new file mode 100644 index 000000000..6de556329 Binary files /dev/null and b/docs/source/frames_demo_2d.jpg differ diff --git a/docs/source/getting_involved.rst b/docs/source/getting_involved.rst new file mode 100644 index 000000000..6ada0854e --- /dev/null +++ b/docs/source/getting_involved.rst @@ -0,0 +1,35 @@ +.. Copyright 2018 The Cartographer Authors + +.. 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. + +================ +Getting involved +================ + +Cartographer is developed in the open and allows anyone to contribute to the project. +There are multiple ways to get involved! + +If you have question or think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. + +.. _GitHub issue: https://github.com/cartographer-project/cartographer/issues + +If you have an idea of a significant change that should be documented and discussed before finding its way into Cartographer, you should submit it as a pull request to `the RFCs repository`_ first. +Simpler changes can also be discussed in GitHub issues so that developers can help you get things right from the first try. + +.. _the RFCs repository: https://github.com/cartographer-project/rfcs + +If you want to contribute code or documentation, this is done through `GitHub pull requests`_. +Pull requests need to follow the `contribution guidelines`_. + +.. _GitHub pull requests: https://github.com/cartographer-project/cartographer/pulls +.. _contribution guidelines: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md diff --git a/docs/source/going_further.rst b/docs/source/going_further.rst new file mode 100644 index 000000000..3e94757ac --- /dev/null +++ b/docs/source/going_further.rst @@ -0,0 +1,80 @@ +.. Copyright 2018 The Cartographer Authors + +.. 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. + +============= +Going further +============= + +Cartographer is not only a great SLAM algorithm, it also comes with a fully-featured implementation that brings lots of "extra" features. +This page lists some of those less known functionalities. + +More input +========== + +If you have a source of odometry (such as a wheel encoder) publishing on a ``nav_msgs/Odometry`` topic and want to use it to improve Cartographer's localization, you can add an input to your ``.lua`` configuration files: + +.. code-block:: lua + + use_odometry = true + +The messages will be expected on the ``odom`` topic. + +A GPS publishing on a ``sensor_msgs/NavSatFix`` topic named ``fix`` can improve the global SLAM: + +.. code-block:: lua + + use_nav_sat = true + +For landmarks publishing on a ``cartographer_ros_msgs/LandmarkList`` (`message defined in cartographer_ros`_) topic named ``landmark``: + +.. code-block:: lua + + use_landmarks = true + +.. _message defined in cartographer_ros: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg + +Localization only +================= + +If you have a map you are happy with and want to reduce computations, you can use the localization-only mode of Cartographer which will run SLAM against the existing map and won't build a new one. +This is enabled by running ``cartographer_node`` with a ``-load_state_filename`` argument and by defining the following line in your lua config: + +.. code-block:: lua + + TRAJECTORY_BUILDER.pure_localization_trimmer = { + max_submaps_to_keep = 3, + } + +IMU Calibration +=============== + +When performing the global optimization, Ceres tries to improve the pose between your IMU and range finding sensors. +A well chosen acquisition with lots of loop closure constraints (for instance if your robot goes on a straight line and then back) can improve the quality of those corrections and become a reliable source of pose correction. +You can then use Cartographer as part of your calibration process to improve the quality of your robot's extrinsic calibration. + +Multi-trajectories SLAM +======================= + +Cartographer can perform SLAM from multiple robots emiting data in parallel. +The global SLAM is able to detect shared paths and will merge the maps built by the different robots as soon as it becomes possible. +This is achieved through the usage of two ROS services ``start_trajectory`` and ``finish_trajectory``. (refer to the ROS API reference documentation for more details on their usage) + +Cloud integration with gRPC +=========================== + +Cartographer is built around Protobuf messages which make it very flexible and interoperable. +One of the advantages of that architecture is that it is easy to distribute on machines spread over the Internet. +The typical use case would be a fleet of robots navigating on a known map, they could have their SLAM algorithm run on a remote powerful centralized localization server running a multi-trajectories Cartographer instance. + +**TODO**: Instructions on how to get started with a gRPC Cartographer instance diff --git a/docs/source/index.rst b/docs/source/index.rst index f9d41c85b..03b85c02f 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -16,94 +16,27 @@ Cartographer ROS Integration ============================ +`Cartographer`_ is a system that provides real-time simultaneous localization +and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor +configurations. This project provides Cartographer's ROS integration. + +.. image:: demo_2d.gif + .. toctree:: :maxdepth: 2 - configuration + compilation + demos + your_bag + algo_walkthrough tuning - ros_api assets_writer - demos + going_further + getting_involved + configuration + ros_api data faq -`Cartographer`_ is a system that provides real-time simultaneous localization -and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor -configurations. This project provides Cartographer's ROS integration. - -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping - -System Requirements -=================== - -See Cartographer's :ref:`system requirements `. - -The following `ROS distributions`_ are currently supported: - -* Indigo -* Kinetic - -.. _ROS distributions: http://wiki.ros.org/Distributions - -Building & Installation -======================= - -We recommend using `wstool `_ and `rosdep -`_. For faster builds, we also recommend using -`Ninja `_. - - .. code-block:: bash - - # Install wstool and rosdep. - sudo apt-get update - sudo apt-get install -y python-wstool python-rosdep ninja-build - - # Create a new workspace in 'catkin_ws'. - mkdir catkin_ws - cd catkin_ws - wstool init src - - # Merge the cartographer_ros.rosinstall file and fetch code for dependencies. - wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall - wstool update -t src - - # Install proto3. - src/cartographer/scripts/install_proto3.sh - - # Install deb dependencies. - # The command 'sudo rosdep init' will print an error if you have already - # executed it since installing ROS. This error can be ignored. - sudo rosdep init - rosdep update - rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y - - # Build and install. - catkin_make_isolated --install --use-ninja - source install_isolated/setup.bash - -Running the demos -================= - -Now that Cartographer and Cartographer's ROS integration are installed, -download the example bags (e.g. 2D and 3D backpack collections of the -`Deutsches Museum `_) to a -known location, in this case ``~/Downloads``, and use ``roslaunch`` to bring up -the demo: - - .. code-block:: bash - - # Download the 2D backpack example bag. - wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag - - # Launch the 2D backpack demo. - roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag - - # Download the 3D backpack example bag. - wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag - - # Launch the 3D backpack demo. - roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag - -The launch files will bring up ``roscore`` and ``rviz`` automatically. -See :doc:`demos` for additional demos including localization and various robots. diff --git a/docs/source/nodes_graph_demo_2d.jpg b/docs/source/nodes_graph_demo_2d.jpg new file mode 100644 index 000000000..19df2f366 Binary files /dev/null and b/docs/source/nodes_graph_demo_2d.jpg differ diff --git a/docs/source/point_cloud_viewer_demo_3d.jpg b/docs/source/point_cloud_viewer_demo_3d.jpg new file mode 100644 index 000000000..90c5bb0f8 Binary files /dev/null and b/docs/source/point_cloud_viewer_demo_3d.jpg differ diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 547350b02..7227d016a 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -12,40 +12,23 @@ See the License for the specific language governing permissions and limitations under the License. -======= -ROS API -======= +=============================== +ROS API reference documentation +=============================== + +.. image:: nodes_graph_demo_2d.jpg Cartographer Node ================= The `cartographer_node`_ is the SLAM node used for online, real-time SLAM. -.. _cartographer_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node_main.cc +.. _cartographer_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node_main.cc Command-line Flags ------------------ -TODO(hrapp): Should these not be removed? It seems duplicated efforts documenting them here and there. - -.. TODO(damonkohler): Use an options list if it can be made to render nicely. - -\-\-configuration_directory - First directory in which configuration files are searched, second is always - the Cartographer installation to allow including files from there. - -\-\-configuration_basename - Basename (i.e. not containing any directory prefix) of the configuration file - (e.g. backpack_3d.lua). - -\-\-load_state_filename - A Cartographer .pbstream state file that will be loaded from disk. This allows - to add new trajectories SLAMing from an earlier state. - -\-\-load_frozen_state - This boolean parameter controls if the saved state, specified using the option - \-\-load_state_filename, is going to be loaded as a set of frozen (not - optimized) trajectories. +Call the node with the ``--help`` flag to see all available options. Subscribed Topics ----------------- @@ -74,7 +57,7 @@ points2 (`sensor_msgs/PointCloud2`_) numbered points2 topics (i.e. points2_1, points2_2, points2_3, ... up to and including *num_point_clouds*) will be used as inputs for SLAM. -The following additional sensor data topics may also be provided. +The following additional sensor data topics may also be provided: imu (`sensor_msgs/Imu`_) Supported in 2D (optional) and 3D (required). This topic will be used as @@ -85,6 +68,8 @@ odom (`nav_msgs/Odometry`_) enabled in the :doc:`configuration`, this topic will be used as input for SLAM. +.. TODO: add NavSatFix? Landmarks? + Published Topics ---------------- @@ -97,6 +82,10 @@ submap_list (`cartographer_ros_msgs/SubmapList`_) List of all submaps, including the pose and latest version number of each submap, across all trajectories. +tracked_pose (`geometry_msgs/PoseStamped`_) + Only published if the parameter ``publish_tracked_pose`` is set to ``true``. + The pose of the tracked frame with respect to the map frame. + Services -------- @@ -109,8 +98,11 @@ submap_query (`cartographer_ros_msgs/SubmapQuery`_) Fetches the requested submap. start_trajectory (`cartographer_ros_msgs/StartTrajectory`_) - Starts another trajectory by specifying its sensor topics and trajectory - options as an binary-encoded proto. Returns an assigned trajectory ID. + Starts a trajectory using default sensor topics and the provided configuration. + An initial pose can be optionally specified. Returns an assigned trajectory ID. + +trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_) + Returns the trajectory data from the pose graph. finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_) Finishes the given `trajectory_id`'s trajectory by running a final optimization. @@ -121,9 +113,19 @@ write_state (`cartographer_ros_msgs/WriteState`_) as input to the `assets_writer_main` to generate assets like probability grids, X-Rays or PLY files. +get_trajectory_states (`cartographer_ros_msgs/GetTrajectoryStates`_) + Returns the IDs and the states of the trajectories. + For example, this can be useful to observe the state of Cartographer from a separate node. + +read_metrics (`cartographer_ros_msgs/ReadMetrics`_) + Returns the latest values of all internal metrics of Cartographer. + The collection of runtime metrics is optional and has to be activated with the ``--collect_metrics`` command line flag in the node. + Required tf Transforms ---------------------- +.. image:: frames_demo_2d.jpg + Transforms from all incoming sensor data frames to the :doc:`configured ` *tracking_frame* and *published_frame* must be available. Typically, these are published periodically by a `robot_state_publisher` or a @@ -133,19 +135,23 @@ Provided tf Transforms ---------------------- The transformation between the :doc:`configured ` *map_frame* -and *published_frame* is always provided. +and *published_frame* is provided unless the parameter ``publish_to_tf`` is set to ``false``. -If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous +If *provide_odom_frame* is enabled in the :doc:`configuration`, additionally a continuous (i.e. unaffected by loop closure) transform between the :doc:`configured ` *odom_frame* and *published_frame* will be provided. .. _robot_state_publisher: http://wiki.ros.org/robot_state_publisher .. _static_transform_publisher: http://wiki.ros.org/tf#static_transform_publisher -.. _cartographer_ros_msgs/FinishTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/FinishTrajectory.srv -.. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg -.. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv -.. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv -.. _cartographer_ros_msgs/WriteState: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv +.. _cartographer_ros_msgs/FinishTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/FinishTrajectory.srv +.. _cartographer_ros_msgs/SubmapList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg +.. _cartographer_ros_msgs/SubmapQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv +.. _cartographer_ros_msgs/StartTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv +.. _cartographer_ros_msgs/TrajectoryQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/TrajectoryQuery.srv +.. _cartographer_ros_msgs/WriteState: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv +.. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv +.. _cartographer_ros_msgs/ReadMetrics: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html @@ -163,17 +169,37 @@ In all other regards, it behaves like the ``cartographer_node``. Each bag will become a separate trajectory in the final state. Once it is done processing all data, it writes out the final Cartographer state and exits. -.. _offline_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/offline_node_main.cc +.. _offline_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/offline_node_main.cc + + +Published Topics +---------------- + +In addition to the topics that are published by the online node, this node also publishes: + +~bagfile_progress (`cartographer_ros_msgs/BagfileProgress`_) + Bag files processing progress including detailed information about the bag currently being processed which will be published with a predefined + interval that can be specified using ``~bagfile_progress_pub_interval`` ROS parameter. + +.. _cartographer_ros_msgs/BagfileProgress: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg + +Parameters +---------- + +~bagfile_progress_pub_interval (double, default=10.0): + The interval of publishing bag files processing progress in seconds. Occupancy grid Node -================= +=================== -The `occupancy_grid_node`_ listens to the submaps published by SLAM and builds a ROS occupancy_grid and publishes it. -This tool is to keep old nodes that require a single monolithic map to work happy until new nav stacks can deal with Cartographer's submaps directly. +The `occupancy_grid_node`_ listens to the submaps published by SLAM, builds an ROS occupancy_grid out of them and publishes it. +This tool is useful to keep old nodes that require a single monolithic map to work happy until new nav stacks can deal with Cartographer's submaps directly. Generating the map is expensive and slow, so map updates are in the order of seconds. +You can can selectively include/exclude submaps from frozen (static) or active trajectories with a command line option. +Call the node with the ``--help`` flag to see these options. -.. _occupancy_grid_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +.. _occupancy_grid_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc Subscribed Topics ----------------- @@ -187,3 +213,23 @@ map (`nav_msgs/OccupancyGrid`_) If subscribed to, the node will continuously compute and publish the map. The time between updates will increase with the size of the map. For faster updates, use the submaps APIs. + + +Pbstream Map Publisher Node +=========================== + +The `pbstream_map_publisher`_ is a simple node that creates a static occupancy grid out of a serialized Cartographer state (pbstream format). +It is an efficient alternative to the occupancy grid node if live updates are not important. + +.. _pbstream_map_publisher: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc + +Subscribed Topics +----------------- + +None. + +Published Topics +---------------- + +map (`nav_msgs/OccupancyGrid`_) + The published occupancy grid topic is latched. diff --git a/docs/source/tuning.rst b/docs/source/tuning.rst index 135d07639..53ad0d315 100644 --- a/docs/source/tuning.rst +++ b/docs/source/tuning.rst @@ -16,35 +16,13 @@ .. cartographer_ros SHA: 99c23b6ac7874f7974e9ed808ace841da6f2c8b0 .. TODO(hrapp): mention insert_free_space somewhere -Tuning -====== +Tuning methodology +================== Tuning Cartographer is unfortunately really difficult. The system has many parameters many of which affect each other. This tuning guide tries to explain a principled approach on concrete examples. -Two systems ------------ - -Cartographer can be seen as two separate, but related systems. -The first one is local SLAM (sometimes also called frontend). -Its job is build a locally consistent set of submaps and tie them together, but it will drift over time. -Most of its options can be found in `trajectory_builder_2d.lua`_ for 2D and `trajectory_builder_3d.lua`_ for 3D. - -.. _trajectory_builder_2d.lua: https://github.com/googlecartographer/cartographer/blob/aba4575d937df4c9697f61529200c084f2562584/configuration_files/trajectory_builder_2d.lua -.. _trajectory_builder_3d.lua: https://github.com/googlecartographer/cartographer/blob/aba4575d937df4c9697f61529200c084f2562584/configuration_files/trajectory_builder_3d.lua - -The other system is global SLAM (sometimes called the backend). -It runs in background threads and its main job is to find loop closure constraints. -It does that by scan-matching scans against submaps. -It also incorporates other sensor data to get a higher level view and identify the most consistent global solution. -In 3D, it also tries to find the direction of gravity. -Most of its options can be found in `pose_graph.lua`_ - -.. _pose_graph.lua: https://github.com/googlecartographer/cartographer/blob/aba4575d937df4c9697f61529200c084f2562584/configuration_files/pose_graph.lua - -On a higher abstraction, the job of local SLAM is to generate good submaps and the job of global SLAM is to tie them most consistently together. - Built-in tools -------------- @@ -68,8 +46,8 @@ It is visible in the laser scan data that contradicting information is passed to But the slipping also indicates that we trust the point cloud matching too much and disregard the other sensors quite strongly. Our aim is to improve the situation through tuning. -.. _aba4575: https://github.com/googlecartographer/cartographer/commit/aba4575d937df4c9697f61529200c084f2562584 -.. _99c23b6: https://github.com/googlecartographer/cartographer_ros/commit/99c23b6ac7874f7974e9ed808ace841da6f2c8b0 +.. _aba4575: https://github.com/cartographer-project/cartographer/commit/aba4575d937df4c9697f61529200c084f2562584 +.. _99c23b6: https://github.com/cartographer-project/cartographer_ros/commit/99c23b6ac7874f7974e9ed808ace841da6f2c8b0 If we only look at this particular submap, that the error is fully contained in one submap. We also see that over time, global SLAM figures out that something weird happened and partially corrects for it. @@ -87,31 +65,9 @@ So let's turn off global SLAM to not mess with our tuning. Correct size of submaps ^^^^^^^^^^^^^^^^^^^^^^^ -Local SLAM drifts over time, only loop closure can fix this drift. -Submaps must be small enough so that the drift inside them is below the resolution, so that they are locally correct. -On the other hand, they should be large enough to be being distinct for loop closure to work properly. The size of submaps is configured through ``TRAJECTORY_BUILDER_2D.submaps.num_range_data``. Looking at the individual submaps for this example they already fit the two constraints rather well, so we assume this parameter is well tuned. -The choice of scan matchers -^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -The idea behind local SLAM is to use sensor data of other sensors besides the range finder to predict where the next scan should be inserted into the submap. -Then, the ``CeresScanMatcher`` takes this as prior and finds the best spot where the scan match fits the submap. -It does this by interpolating the submap and sub-pixel aligning the scan. -This is fast, but cannot fix errors that are significantly larger than the resolution of the submaps. -If your sensor setup and timing is reasonable, using only the ``CeresScanMatcher`` is usually the best choice to make. - -If you do not have other sensors or you do not trust them, Cartographer also provides a ``RealTimeCorrelativeScanMatcher``. -It uses an approach similar to how scans are matched against submaps in loop closure, but instead it matches against the current submap. -The best match is then used as prior for the ``CeresScanMatcher``. -This scan matcher is very expensive and will essentially override any signal from other sensors but the range finder, but it is robust in feature rich environments. - -Tuning the correlative scan matcher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -TODO - Tuning the ``CeresScanMatcher`` ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -147,7 +103,7 @@ Before checking them in, we normalize all weights, since they only have relative The result of this tuning was `PR 428`_. In general, always try to tune for a platform, not a particular bag. -.. _PR 428: https://github.com/googlecartographer/cartographer/pull/428 +.. _PR 428: https://github.com/cartographer-project/cartographer/pull/428 Special Cases ------------- @@ -225,7 +181,7 @@ If a separate odometry source is used as an input for local SLAM (``use_odometry There are in total four parameters that allow us to tune the individual weights of local SLAM and odometry in the optimization: - .. code-block:: lua +.. code-block:: lua POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight @@ -236,3 +192,18 @@ We can set these weights depending on how much we trust either local SLAM or the By default, odometry is weighted into global optimization similar to local slam (scan matching) poses. However, odometry from wheel encoders often has a high uncertainty in rotation. In this case, the rotation weight can be reduced, even down to zero. + +Still have a problem ? +---------------------- + +If you can't get Cartographer to work reliably on your data, you can open a `GitHub issue`_ asking for help. +Developers are keen to help, but they can only be helpful if you follow `an issue template`_ containing the result of ``rosbag_validate``, a link to a fork of ``cartographer_ros`` with your config and a link to a ``.bag`` file reproducing your problem. + +.. note:: + + There are already lots of GitHub issues with all sorts of problems solved by the developers. Going through `the closed issues of cartographer_ros`_ and `of cartographer`_ is a great way to learn more about Cartographer and maybe find a solution to your problem ! + +.. _GitHub issue: https://github.com/cartographer-project/cartographer_ros/issues +.. _an issue template: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question +.. _the closed issues of cartographer_ros: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed +.. _of cartographer: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed diff --git a/docs/source/your_bag.rst b/docs/source/your_bag.rst new file mode 100644 index 000000000..8d33e0fd0 --- /dev/null +++ b/docs/source/your_bag.rst @@ -0,0 +1,142 @@ +.. Copyright 2018 The Cartographer Authors + +.. 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. + +======================================== +Running Cartographer ROS on your own bag +======================================== + +Now that you've run Cartographer ROS on a couple of provided bags, you can go ahead and make Cartographer work with your own data. +Find a ``.bag`` recording you would like to use for SLAM and go through this tutorial. + +.. warning:: When you want to run cartographer_ros, you might need to source your ROS environment by running ``source install_isolated/setup.bash`` first (replace bash with zsh if your shell is zsh) + +Validate your bag +================= + +Cartographer ROS provides a tool named ``cartographer_rosbag_validate`` to automatically analyze data present in your bag. +It is generally a good idea to run this tool before trying to tune Cartographer for incorrect data. + +It benefits from the experience of the Cartographer authors and can detect a variety of mistakes commonly found in bags. +For instance, if a ``sensor_msgs/Imu`` topic is detected, the tool will make sure that the gravity vector has not been removed from the IMU measurements because the gravity norm is used by Cartographer to determine the direction of the ground. + +The tool can also provide tips on how to improve the quality of your data. +For example, with a Velodyne LIDAR, it is recommended to have one ``sensor_msgs/PointCloud2`` message per UDP packet sent by the sensor instead of one message per revolution. +With that granularity, Cartographer is then able to unwarp the point clouds deformation caused by the robot's motion and results in better reconstruction. + +If you have sourced your Cartographer ROS environment, you can simply run the tool like this: + +.. code-block:: bash + + cartographer_rosbag_validate -bag_filename your_bag.bag + +Create a .lua configuration +=========================== + +Cartographer is highly flexible and can be configured to work on a variety of robots. +The robot configuration is read from a ``options`` data structure that must be defined from a Lua script. +The example configurations are defined in ``src/cartographer_ros/cartographer_ros/configuration_files`` and installed in ``install_isolated/share/cartographer_ros/configuration_files/``. + +.. note:: Ideally, a .lua configuration should be robot-specific and not bag-specific. + +You can start by copying one of the example and then adapt it to your own need. If you want to use 3D SLAM: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/configuration_files/backpack_3d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua + +If you want to use 2D SLAM: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua + +You can then edit ``my_robot.lua`` to suit the needs of your robot. +The values defined in the ``options`` block define how the Cartographer ROS frontend should interface with your bag. +The values defined after the ``options`` paragraph are used to tune the inner-working of Cartographer, we will ignore these for now. + +.. seealso:: The `reference documentation of the Cartographer ROS configuration values`_ and of `the Cartographer configuration values`_. + +.. _reference documentation of the Cartographer ROS configuration values: https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html + +.. _the Cartographer configuration values: https://google-cartographer.readthedocs.io/en/latest/configuration.html + +Among the values you need to adapt, you probably have to provide the TF frame IDs of your environment and robot in ``map_frame``, ``tracking_frame``, ``published_frame`` and ``odom_frame``. + +.. note:: You can either distribute your robot's TF tree from a ``/tf`` topic in your bag or define it in a ``.urdf`` robot definition. + +.. warning:: You should trust your poses! A small offset on the link between your robot and IMU or LIDAR can lead to incoherent map reconstructions. Cartographer can usually correct small pose errors but not everything! + +The other values you need to define are related to the number and type of sensors you would like to use. + +- ``num_laser_scans``: Number of ``sensor_msgs/LaserScan`` topics you'll use. +- ``num_multi_echo_laser_scans``: Number of ``sensor_msgs/MultiEchoLaserScan`` topics you'll use. +- ``num_point_clouds``: Number of ``sensor_msgs/PointCloud2`` topics you'll use. + +You can also enable the usage of landmarks and GPS as additional sources of localization using ``use_landmarks`` and ``use_nav_sat``. The rest of the variables in the ``options`` block should typically be left untouched. + +.. note:: even if you use a 2D SLAM, the landmarks are 3D objects and can mislead you if viewed only on the 2D plane due to their third dimension. + +However, there is one global variable that you absolutely need to adapt to the needs of your bag: ``TRAJECTORY_BUILDER_3D.num_accumulated_range_data`` or ``TRAJECTORY_BUILDER_2D.num_accumulated_range_data``. +This variable defines the number of messages required to construct a full scan (typically, a full revolution). +If you follow ``cartographer_rosbag_validate``'s advices and use 100 ROS messages per scan, you can set this variable to 100. +If you have two range finding sensors (for instance, two LIDARs) providing their full scans all at once, you should set this variable to 2. + +Create .launch files for your SLAM scenarios +============================================ + +You may have noticed that each demo introduced in the previous section was run with a different roslaunch command. +The recommended usage of Cartographer is indeed to provide a custom ``.launch`` file per robot and type of SLAM. +The example ``.launch`` files are defined in ``src/cartographer_ros/cartographer_ros/launch`` and installed in ``install_isolated/share/cartographer_ros/launch/``. + +Start by copying one of the provided example: + +.. code-block:: bash + + cp install_isolated/share/cartographer_ros/launch/backpack_3d.launch install_isolated/share/cartographer_ros/launch/my_robot.launch + cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d.launch install_isolated/share/cartographer_ros/launch/demo_my_robot.launch + cp install_isolated/share/cartographer_ros/launch/offline_backpack_3d.launch install_isolated/share/cartographer_ros/launch/offline_my_robot.launch + cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d_localization.launch install_isolated/share/cartographer_ros/launch/demo_my_robot_localization.launch + cp install_isolated/share/cartographer_ros/launch/assets_writer_backpack_3d.launch install_isolated/share/cartographer_ros/launch/assets_writer_my_robot.launch + +- ``my_robot.launch`` is meant to be used on the robot to execute SLAM online (in real time) with real sensors data. +- ``demo_my_robot.launch`` is meant to be used from a development machine and expects a ``bag_filename`` argument to replay data from a recording. This launch file also spawns a rviz window configured to visualize Cartographer's state. +- ``offline_my_robot.launch`` is very similar to ``demo_my_robot.launch`` but tries to execute SLAM as fast as possible. This can make map building significantly faster. This launch file can also use multiple bag files provided to the ``bag_filenames`` argument. +- ``demo_my_robot_localization.launch`` is very similar to ``demo_my_robot.launch`` but expects a ``load_state_filename`` argument pointing to a ``.pbstream`` recording of a previous Cartographer execution. The previous recording will be used as a pre-computed map and Cartographer will only perform localization on this map. +- ``assets_writer_my_robot.launch`` is used to extract data out of a ``.pbstream`` recording of a previous Cartographer execution. + +Again, a few adaptations need to be made to those files to suit your robot. + +- Every parameter given to ``-configuration_basename`` should be adapted to point to ``my_robot.lua``. +- If you decided to use a ``.urdf`` description of your robot, you should place your description in ``install_isolated/share/cartographer_ros/urdf`` and adapt the ``robot_description`` parameter to point to your file name. +- If you decided to use ``/tf`` messages, you can remove the ``robot_description`` parameter, the ``robot_state_publisher`` node and the lines statring with ``-urdf``. +- If the topic names published by your bag or sensors don't match the ones expected by Cartographer ROS, you can use ```` elements to redirect your topics. The expected topic names depend on the type of range finding devices you use. + +.. note:: + + - The IMU topic is expected to be named "imu" + - If you use only one ``sensor_msgs/LaserScan`` topic, it is expected to be named ``scan``. If you have more, they should be named ``scan_1``, ``scan_2`` etc... + - If you use only one ``sensor_msgs/MultiEchoLaserScan`` topic, it is expected to be named ``echoes``. If you have more, they should be named ``echoes_1``, ``echoes_2`` etc... + - If you use only one ``sensor_msgs/PointCloud2`` topic, it is expected be named ``points2``. If you have more, they should be named ``points2_1``, ``points2_2``, etc... + +Try your configuration +====================== + +Everything is setup! You can now start Cartographer with: + +.. code-block:: bash + + roslaunch cartographer_ros my_robot.launch bag_filename:=/path/to/your_bag.bag + +If you are lucky enough, everything should already work as expected. +However, you might have some problems that require tuning. diff --git a/jenkins/Dockerfile.kinetic b/jenkins/Dockerfile.kinetic deleted file mode 100644 index 227aa79f3..000000000 --- a/jenkins/Dockerfile.kinetic +++ /dev/null @@ -1,122 +0,0 @@ -# Copyright 2016 The Cartographer Authors -# -# 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. - -FROM ros:kinetic - -ARG CARTOGRAPHER_VERSION=master - -# Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo time && rm -rf /var/lib/apt/lists/* -# First, we invalidate the entire cache if googlecartographer/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_jenkins_catkin_workspace.sh cartographer_ros/scripts/ - -# Invalidates the Docker cache to ensure this command is always executed. -ARG CACHEBUST=1 -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_jenkins_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - -# Build, install, and test all packages individually to allow caching. The -# ordering of these steps must match the topological package ordering as -# determined by Catkin. -COPY scripts/install.sh cartographer_ros/scripts/ -COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ - -COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs - -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - -COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros - -COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ - cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz - -RUN cartographer_ros/scripts/install.sh --pkg cartographer_toru -RUN cartographer_ros/scripts/install.sh --pkg cartographer_fetch - -COPY scripts/ros_entrypoint.sh / -# A BTRFS bug may prevent us from cleaning up these directories. -# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory -RUN rm -rf cartographer_ros catkin_ws || true - -RUN sudo apt-get update -RUN sudo apt-get -y install openjdk-8-jdk python-pip - -ENV HOME /home/jenkins -RUN addgroup --system --gid 10000 jenkins -RUN adduser --system --ingroup jenkins --home $HOME --uid 10000 jenkins - -LABEL Description="This is a base image, which provides the Jenkins agent executable (slave.jar)" Vendor="Jenkins project" Version="3.17" - -ARG VERSION=3.17 -ARG AGENT_WORKDIR=/home/jenkins/agent - -RUN curl --create-dirs -sSLo /usr/share/jenkins/slave.jar https://repo.jenkins-ci.org/public/org/jenkins-ci/main/remoting/${VERSION}/remoting-${VERSION}.jar \ - && chmod 755 /usr/share/jenkins \ - && chmod 644 /usr/share/jenkins/slave.jar -# USER jenkins -ENV AGENT_WORKDIR=${AGENT_WORKDIR} -RUN mkdir /home/jenkins/.jenkins && mkdir -p ${AGENT_WORKDIR} - -VOLUME /home/jenkins/.jenkins -VOLUME ${AGENT_WORKDIR} -WORKDIR /home/jenkins - -COPY jenkins/jenkins-slave /usr/local/bin/jenkins-slave - -ENV CLOUDSDK_CORE_DISABLE_PROMPTS 1 -ENV PATH /opt/google-cloud-sdk/bin:$PATH - -USER root - -# Install Google Cloud Components -RUN curl https://sdk.cloud.google.com | bash && mv google-cloud-sdk /opt -RUN gcloud components install kubectl - -RUN pip install --upgrade google-cloud-datastore -RUN pip install --upgrade google-cloud-bigquery -COPY jenkins/worker.py /worker.py - -# USER root -ENTRYPOINT ["jenkins-slave"] diff --git a/jenkins/Jenkinsfile b/jenkins/Jenkinsfile deleted file mode 100644 index 06bb2fcf4..000000000 --- a/jenkins/Jenkinsfile +++ /dev/null @@ -1,110 +0,0 @@ -podTemplate(label: 'node-0', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eggsy84/gcp-jenkins-slave-k8s-seed:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: false, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-0') { - stage('Compile') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'cd /data && rm -Rf *' - sh 'cd /data && git clone https://github.com/googlecartographer/cartographer_ros' - sh 'cd /data/cartographer_ros && docker build -f jenkins/Dockerfile.kinetic -t kinetic-jenkins-slave --build-arg CACHEBUST=$(date +%s) .' - } - stage('Push') { - sh 'docker tag kinetic-jenkins-slave eu.gcr.io/cartographer-141408/kinetic-jenkins-slave' - sh 'gcloud docker -- push eu.gcr.io/cartographer-141408/kinetic-jenkins-slave' - sh 'cd /data && rm -Rf *' - } - } -} - -podTemplate(label: 'node-1', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-1') { - stage('Run Fetch Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id fetch' - } - } -} - -podTemplate(label: 'node-2', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-2') { - stage('Run Backpack Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id backpack' - } - } -} - -podTemplate(label: 'node-3', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-3') { - stage('Run Toru Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id toru' - } - } -} diff --git a/jenkins/jenkins-slave b/jenkins/jenkins-slave deleted file mode 100755 index bf73566ae..000000000 --- a/jenkins/jenkins-slave +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env sh - -# The MIT License -# -# Copyright (c) 2015, CloudBees, Inc. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -# Usage jenkins-slave.sh [options] -url http://jenkins [SECRET] [AGENT_NAME] -# Optional environment variables : -# * JENKINS_TUNNEL : HOST:PORT for a tunnel to route TCP traffic to jenkins host, when jenkins can't be directly accessed over network -# * JENKINS_URL : alternate jenkins URL -# * JENKINS_SECRET : agent secret, if not set as an argument -# * JENKINS_AGENT_NAME : agent name, if not set as an argument - -if [ $# -eq 1 ]; then - - # if `docker run` only has one arguments, we assume user is running alternate command like `bash` to inspect the image - exec "$@" - -else - - # if -tunnel is not provided try env vars - case "$@" in - *"-tunnel "*) ;; - *) - if [ ! -z "$JENKINS_TUNNEL" ]; then - TUNNEL="-tunnel $JENKINS_TUNNEL" - fi ;; - esac - - if [ -n "$JENKINS_URL" ]; then - URL="-url $JENKINS_URL" - fi - - if [ -n "$JENKINS_NAME" ]; then - JENKINS_AGENT_NAME="$JENKINS_NAME" - fi - - if [ -z "$JNLP_PROTOCOL_OPTS" ]; then - echo "Warning: JnlpProtocol3 is disabled by default, use JNLP_PROTOCOL_OPTS to alter the behavior" - JNLP_PROTOCOL_OPTS="-Dorg.jenkinsci.remoting.engine.JnlpProtocol3.disabled=true" - fi - - # If both required options are defined, do not pass the parameters - OPT_JENKINS_SECRET="" - if [ -n "$JENKINS_SECRET" ]; then - case "$@" in - *"${JENKINS_SECRET}"*) echo "Warning: SECRET is defined twice in command-line arguments and the environment variable" ;; - *) - OPT_JENKINS_SECRET="${JENKINS_SECRET}" ;; - esac - fi - - OPT_JENKINS_AGENT_NAME="" - if [ -n "$JENKINS_AGENT_NAME" ]; then - case "$@" in - *"${JENKINS_AGENT_NAME}"*) echo "Warning: AGENT_NAME is defined twice in command-line arguments and the environment variable" ;; - *) - OPT_JENKINS_AGENT_NAME="${JENKINS_AGENT_NAME}" ;; - esac - fi - - #TODO: Handle the case when the command-line and Environment variable contain different values. - #It is fine it blows up for now since it should lead to an error anyway. - - exec java $JAVA_OPTS $JNLP_PROTOCOL_OPTS -cp /usr/share/jenkins/slave.jar hudson.remoting.jnlp.Main -headless $TUNNEL $URL $OPT_JENKINS_SECRET $OPT_JENKINS_AGENT_NAME "$@" -fi diff --git a/jenkins/worker.py b/jenkins/worker.py deleted file mode 100644 index e23213e34..000000000 --- a/jenkins/worker.py +++ /dev/null @@ -1,350 +0,0 @@ -"""This is the script executed by workers of the quality control pipline.""" - -import argparse -import datetime -from os.path import basename -from pprint import pprint -import re -import subprocess - -from google.cloud import bigquery -from google.cloud import datastore - - -class Pattern(object): - """Defines a pattern for regular expression matching.""" - - def __init__(self, pattern): - self.regex = re.compile(pattern, re.MULTILINE) - - def extract(self, text): - """Returns a dictionary of named capture groups to extracted output. - - Args: - text: input to parse - - Returns an empty dict if no match was found. - """ - match = self.regex.search(text) - if match is None: - return {} - return match.groupdict() - - def extract_last_occurence(self, text): - """Returns tuple of extracted outputs. - - Args: - text: input to parse - - Returns the information extracted from the last match. Returns - None if no match was found. - """ - matches = self.regex.findall(text) - if not matches: - return None - return matches[-1] - - -# BigQuery table schema -SCHEMA = [ - bigquery.SchemaField('date', 'DATE'), - bigquery.SchemaField('commit_sha1', 'STRING'), - bigquery.SchemaField('job_id', 'INTEGER'), - bigquery.SchemaField('rosbag', 'STRING'), - bigquery.SchemaField('user_time_secs', 'FLOAT'), - bigquery.SchemaField('system_time_secs', 'FLOAT'), - bigquery.SchemaField('wall_time_secs', 'FLOAT'), - bigquery.SchemaField('max_set_size_kbytes', 'INTEGER'), - bigquery.SchemaField('constraints_count', 'INTEGER'), - bigquery.SchemaField('constraints_score_minimum', 'FLOAT'), - bigquery.SchemaField('constraints_score_maximum', 'FLOAT'), - bigquery.SchemaField('constraints_score_mean', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_trans_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_trans_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_trans_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_trans_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_rot_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_rot_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_rot_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_rot_err_dev', 'FLOAT') -] - -# Pattern matchers for the various fields of the '/usr/bin/time -v' output -USER_TIME_PATTERN = Pattern( - r'^\s*User time \(seconds\): (?P\d+.\d+|\d+)') -SYSTEM_TIME_PATTERN = Pattern( - r'^\s*System time \(seconds\): (?P\d+.\d+|\d+)') -WALL_TIME_PATTERN = Pattern( - r'^\s*Elapsed \(wall clock\) time \(h:mm:ss or m:ss\): ' - r'((?P\d{1,2}):|)(?P\d{1,2}):(?P\d{2}\.\d{2})') -MAX_RES_SET_SIZE_PATTERN = Pattern( - r'^\s*Maximum resident set size \(kbytes\): (?P\d+)') -CONSTRAINT_STATS_PATTERN = Pattern( - r'Score histogram:[\n\r]+' - r'Count:\s+(?P\d+)\s+' - r'Min:\s+(?P\d+\.\d+)\s+' - r'Max:\s+(?P\d+\.\d+)\s+' - r'Mean:\s+(?P\d+\.\d+)') -GROUND_TRUTH_STATS_PATTERN = Pattern( - r'Result:[\n\r]+' - r'Abs translational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) m[\n\r]+' - r'Sqr translational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) m\^2[\n\r]+' - r'Abs rotational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) deg[\n\r]+' - r'Sqr rotational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) deg\^2') - -# Pattern matcher for extracting the HEAD commit SHA-1 hash. -GIT_SHA1_PATTERN = Pattern(r'^(?P[0-9a-f]{40})\s+HEAD') - - -def get_head_git_sha1(): - """Returns the SHA-1 hash of the commit tagged HEAD.""" - output = subprocess.check_output([ - 'git', 'ls-remote', - 'https://github.com/googlecartographer/cartographer.git' - ]) - parsed = GIT_SHA1_PATTERN.extract(output) - return parsed['sha1'] - - -def extract_stats(inp): - """Returns a dictionary of stats.""" - result = {} - - parsed = USER_TIME_PATTERN.extract(inp) - result['user_time_secs'] = float(parsed['user_time']) - - parsed = SYSTEM_TIME_PATTERN.extract(inp) - result['system_time_secs'] = float(parsed['system_time']) - - parsed = WALL_TIME_PATTERN.extract(inp) - result['wall_time_secs'] = float(parsed['hours'] or 0.) * 3600 + float( - parsed['minutes']) * 60 + float(parsed['seconds']) - - parsed = MAX_RES_SET_SIZE_PATTERN.extract(inp) - result['max_set_size_kbytes'] = int(parsed['max_set_size']) - - parsed = CONSTRAINT_STATS_PATTERN.extract_last_occurence(inp) - print parsed - result['constraints_count'] = int(parsed[0]) - result['constraints_score_min'] = float(parsed[1]) - result['constraints_score_max'] = float(parsed[2]) - result['constraints_score_mean'] = float(parsed[3]) - - return result - - -def extract_ground_truth_stats(input_text): - """Returns a dictionary of ground truth stats.""" - result = {} - parsed = GROUND_TRUTH_STATS_PATTERN.extract(input_text) - for name in ('abs_trans_err', 'sqr_trans_err', 'abs_rot_err', 'sqr_rot_err'): - result['ground_truth_{}'.format(name)] = float(parsed[name]) - result['ground_truth_{}_dev'.format(name)] = float( - parsed['{}_dev'.format(name)]) - return result - - -def retrieve_entity(datastore_client, kind, identifier): - """Convenience function for Datastore entity retrieval.""" - key = datastore_client.key(kind, identifier) - return datastore_client.get(key) - - -def create_job_selector(worker_id, num_workers): - """Constructs a round-robin job selector.""" - return lambda job_id: job_id % num_workers == worker_id - - -def run_cmd(cmd): - """Runs command both printing its stdout output and returning it as string.""" - print cmd - p = subprocess.Popen( - cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) - run_cmd.output = [] - - def process(line): - run_cmd.output.append(line) - print line.rstrip() - - while p.poll() is None: - process(p.stdout.readline()) - process(p.stdout.read()) - return '\n'.join(run_cmd.output) - - -def run_ros_cmd(ros_distro, ros_cmd): - """Runs command similar to run_cmd but sets ROS environment variables.""" - cmd = ('/bin/bash -c \"source /opt/ros/{}/setup.bash && source ' - '/opt/cartographer_ros/setup.bash && {}\"').format( - ros_distro, ros_cmd) - return run_cmd(cmd) - - -class Job(object): - """Represents a single job to be executed. - - A job consists of a combination of rosbag and configuration and launch files. - """ - - def __init__(self, datastore_client, job_id): - self.id = job_id - entity = retrieve_entity(datastore_client, 'Job', job_id) - self.launch_file = entity['launch_file'] - self.assets_writer_launch_file = entity['assets_writer_launch_file'] - self.assets_writer_config_file = entity['assets_writer_config_file'] - self.rosbag = entity['rosbag'] - self.ros_package = entity['ros_package'] - - def __repr__(self): - return 'Job: id : {} launch_file: {} rosbag: {}'.format( - self.id, self.launch_file, self.rosbag) - - def run(self, ros_distro, run_id): - """Runs the job with ROS distro 'ros_distro'.""" - print 'running job {}'.format(self.id) - - # Garbage collect any left-overs from previous runs. - run_cmd('rm -rf /data/*') - - # Copy the rosbag to scratch space - scratch_dir = '/data/{}'.format(self.id) - rosbag_filename = basename(self.rosbag) - run_cmd('mkdir {}'.format(scratch_dir)) - run_cmd('gsutil cp gs://{} {}/{}'.format(self.rosbag, scratch_dir, - rosbag_filename)) - - # Creates pbstream - output = run_ros_cmd(ros_distro, - '/usr/bin/time -v roslaunch {} {} ' - 'bag_filenames:={}/{} no_rviz:=true'.format( - self.ros_package, self.launch_file, scratch_dir, - rosbag_filename)) - info = extract_stats(output) - - # Creates assets. - run_ros_cmd( - ros_distro, '/usr/bin/time -v roslaunch {} {} ' - 'bag_filenames:={}/{} pose_graph_filename:=' - '{}/{}.pbstream config_file:={}'.format( - self.ros_package, self.assets_writer_launch_file, scratch_dir, - rosbag_filename, scratch_dir, rosbag_filename, - self.assets_writer_config_file)) - - # Copies assets to bucket. - run_cmd('gsutil cp {}/{}.pbstream ' - 'gs://cartographer-ci-artifacts/{}/{}/{}.pbstream'.format( - scratch_dir, rosbag_filename, run_id, self.id, rosbag_filename)) - run_cmd('gsutil cp {}/{}_* gs://cartographer-ci-artifacts/{}/{}/'.format( - scratch_dir, rosbag_filename, run_id, self.id)) - - # Download ground truth relations file. - run_cmd('gsutil cp gs://cartographer-ci-ground-truth/{}/relations.pb ' - '{}/relations.pb'.format(self.id, scratch_dir)) - - # Calculate metrics. - output = run_ros_cmd(ros_distro, 'cartographer_compute_relations_metrics ' - '-relations_filename {}/relations.pb ' - '-pose_graph_filename {}/{}.pbstream'.format( - scratch_dir, scratch_dir, rosbag_filename)) - - # Add ground truth stats. - info.update(extract_ground_truth_stats(output)) - - info['rosbag'] = rosbag_filename - return info - - -class Worker(object): - """Represents a single worker that executes a sequence of Jobs.""" - - def __init__(self, datastore_client, pipeline_id, run_id): - entity = retrieve_entity(datastore_client, 'PipelineConfig', pipeline_id) - self.pipeline_id = pipeline_id - self.jobs = [Job(datastore_client, job_id) for job_id in entity['jobs']] - self.scratch_dir = entity['scratch_dir'] - self.ros_distro = entity['ros_distro'] - self.run_id = run_id - - def __repr__(self): - result = 'Worker: pipeline_id: {}\n'.format(self.pipeline_id) - for job in self.jobs: - result += '{}\n'.format(str(job)) - return result - - def run_jobs(self, selector): - outputs = {} - for idx, job in enumerate(self.jobs): - if selector(idx): - output = job.run(self.ros_distro, self.run_id) - outputs[job.id] = output - else: - print 'job {}: skip'.format(job.id) - return outputs - - -def publish_stats_to_big_query(stats_dict, now, head_sha1): - """Publishes metrics to BigQuery.""" - bigquery_client = bigquery.Client() - dataset = bigquery_client.dataset('Cartographer') - table = dataset.table('metrics') - rows_to_insert = [] - for job_identifier, job_info in stats_dict.iteritems(): - print job_info - data = ('{}-{}-{}'.format( - now.year, now.month, - now.day), head_sha1, job_identifier, job_info['rosbag'], - job_info['user_time_secs'], job_info['system_time_secs'], - job_info['wall_time_secs'], job_info['max_set_size_kbytes'], - job_info['constraints_count'], job_info['constraints_score_min'], - job_info['constraints_score_max'], - job_info['constraints_score_mean'], - job_info['ground_truth_abs_trans_err'], - job_info['ground_truth_abs_trans_err_dev'], - job_info['ground_truth_sqr_trans_err'], - job_info['ground_truth_sqr_trans_err_dev'], - job_info['ground_truth_abs_rot_err'], - job_info['ground_truth_abs_rot_err_dev'], - job_info['ground_truth_sqr_rot_err'], - job_info['ground_truth_sqr_rot_err_dev']) - - rows_to_insert.append(data) - - errors = bigquery_client.create_rows( - table, rows_to_insert, selected_fields=SCHEMA) - if not errors: - print 'Pushed {} row(s) into Cartographer:metrics'.format( - len(rows_to_insert)) - else: - print 'Errors:' - pprint(errors) - - -def parse_arguments(): - """Parses the command line arguments.""" - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument('--worker_id', type=int) - parser.add_argument('--num_workers', type=int) - parser.add_argument('--pipeline_id', type=str) - return parser.parse_args() - - -def main(): - args = parse_arguments() - ds_client = datastore.Client() - job_selector = create_job_selector(int(args.worker_id), int(args.num_workers)) - head_sha1 = get_head_git_sha1() - now = datetime.datetime.now() - pipeline_run_id = '{}-{}-{}_{}'.format(now.year, now.month, now.day, - head_sha1) - worker = Worker(ds_client, args.pipeline_id, pipeline_run_id) - stats_dict = worker.run_jobs(job_selector) - publish_stats_to_big_query(stats_dict, now, head_sha1) - - -if __name__ == '__main__': - main() diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index 8291bc542..0af46489a 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash # Copyright 2016 The Cartographer Authors # @@ -17,14 +17,23 @@ set -o errexit set -o verbose +# Install CMake, Ninja, stow. +sudo apt-get update +sudo apt-get install -y lsb-release cmake ninja-build stow + +# Install GMock library and header files for newer distributions. +if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] +then + sudo apt-get install -y libgmock-dev +fi + . /opt/ros/${ROS_DISTRO}/setup.sh cd catkin_ws -# Install Ninja. -apt-get update -apt-get install -y ninja-build - # Install rosdep dependencies. rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y + +# Update rosconsole-bridge to fix build issue with Docker image for Kinetic +sudo apt-get install ros-${ROS_DISTRO}-rosconsole-bridge -y diff --git a/scripts/prepare_catkin_workspace.sh b/scripts/prepare_catkin_workspace.sh index 7e38d2475..77b870ee5 100755 --- a/scripts/prepare_catkin_workspace.sh +++ b/scripts/prepare_catkin_workspace.sh @@ -26,6 +26,10 @@ wstool init # Merge the cartographer_ros.rosinstall file and fetch code for dependencies. wstool merge ../../cartographer_ros/cartographer_ros.rosinstall -wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +# We default to master and wstool seems to have a bug when it's being set twice. +# TODO(MichaelGrupp): wstool is unmaintained, use vcstool. +if [ ${CARTOGRAPHER_VERSION} != "master" ]; then + wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +fi wstool remove cartographer_ros wstool update diff --git a/scripts/prepare_jenkins_catkin_workspace.sh b/scripts/prepare_jenkins_catkin_workspace.sh index 1db3b0644..9c9654210 100755 --- a/scripts/prepare_jenkins_catkin_workspace.sh +++ b/scripts/prepare_jenkins_catkin_workspace.sh @@ -26,7 +26,7 @@ wstool init # Merge the cartographer_ros.rosinstall file and fetch code for dependencies. wstool merge ../../cartographer_ros/cartographer_ros.rosinstall -wstool merge -y https://raw.githubusercontent.com/googlecartographer/cartographer_fetch/master/cartographer_fetch.rosinstall +wstool merge -y https://raw.githubusercontent.com/cartographer-project/cartographer_fetch/master/cartographer_fetch.rosinstall wstool merge -y https://raw.githubusercontent.com/magazino/cartographer_magazino/master/cartographer_magazino.rosinstall wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y wstool remove cartographer_ros