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.base.melodic b/Dockerfile.base.melodic deleted file mode 100644 index 4b2274fdd..000000000 --- a/Dockerfile.base.melodic +++ /dev/null @@ -1,43 +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. - -FROM ros:melodic - -ARG CARTOGRAPHER_VERSION=master - -# Bionic's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && 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_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 - 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 86523f7ca..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 eu.gcr.io/cartographer-141408/cartographer_ros_melodic_base +FROM osrf/ros:melodic-desktop ARG CARTOGRAPHER_VERSION=master @@ -20,26 +20,29 @@ 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. COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/update_catkin_workspace.sh cartographer_ros/scripts/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/update_catkin_workspace.sh + 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/* +RUN cartographer_ros/scripts/install_debs.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 @@ -47,17 +50,15 @@ RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* 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 \ @@ -71,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 index e359dd092..a778158c9 100644 --- a/azure-pipelines.yml +++ b/azure-pipelines.yml @@ -25,7 +25,7 @@ jobs: 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/googlecartographer/cartographer src\cartographer + 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' 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/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index a379726b7..306a660d7 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. @@ -12,185 +13,201 @@ # 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_ros) -set(PACKAGE_DEPENDENCIES +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_POSITION_INDEPENDENT_CODE ON) + +find_package(cartographer REQUIRED) +find_package(cartographer_ros_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(rosbag2_compression REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_storage REQUIRED) +find_package(absl REQUIRED) +find_package(LuaGoogle REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(Eigen3 REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom_headers REQUIRED) + +include_directories( + include + ${LUA_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} +) + +# 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 - eigen_conversions geometry_msgs - message_runtime nav_msgs pcl_conversions - rosbag - roscpp - roslib + rclcpp sensor_msgs std_msgs tf2 tf2_eigen + tf2_msgs tf2_ros - urdf visualization_msgs + rosbag2_compression + rosbag2_cpp + rosbag2_storage + urdf + urdfdom ) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} + ) +target_link_libraries(${PROJECT_NAME} cartographer ${PCL_LIBRARIES}) -if(WIN32) - set(Boost_USE_STATIC_LIBS FALSE) -endif() -# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer), -# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS, -# which causes linking problems on windows due to shared/static Thread clashing. -# PCL also finds Boost. Work around by moving before find_package(cartographer). -find_package(Boost REQUIRED COMPONENTS system iostreams) -find_package(PCL REQUIRED COMPONENTS common) +# Executables +add_executable(cartographer_node src/node_main.cpp) +target_link_libraries(cartographer_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_node ${dependencies}) -find_package(cartographer REQUIRED) -include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") -option(BUILD_GRPC "build features that require Cartographer gRPC support" false) -google_initialize_cartographer_project() -google_enable_testing() +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}) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_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}) -include(FindPkgConfig) +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}) -find_package(Abseil REQUIRED) -find_package(LuaGoogle REQUIRED) -find_package(Eigen3 REQUIRED) +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}) -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() +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}) -include_directories( - ${urdfdom_headers_INCLUDE_DIRS} -) +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}) -# Override Catkin's GTest configuration to use GMock. -set(GTEST_FOUND TRUE) -set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS}) -set(GTEST_LIBRARIES ${GMOCK_LIBRARIES}) - -catkin_package( - CATKIN_DEPENDS - ${PACKAGE_DEPENDENCIES} - DEPENDS - # TODO(damonkohler): This should be here but causes Catkin to abort because - # protobuf specifies a library '-lpthread' instead of just 'pthread'. - # CARTOGRAPHER - PCL - EIGEN3 - Boost - urdfdom_headers - INCLUDE_DIRS "." - LIBRARIES ${PROJECT_NAME} -) -file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h") -file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc") -file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc") -file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*") -list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS}) -list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES}) -if (NOT ${BUILD_GRPC}) - list(REMOVE_ITEM ALL_SRCS ${ALL_GRPC_FILES}) - list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES}) - list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES}) -endif() -add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) -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}") - -# Boost -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${Boost_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) - -# Catkin -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 the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - -if (CATKIN_ENABLE_TESTING) - foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) - get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) - catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) - # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to - # target_link_libraries. That forces us to do the same. - target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) - target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) - add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${TEST_NAME} cartographer) - target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) - set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - # Needed for dynamically linked GTest on Windows. - if (WIN32) - target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) - endif() - endforeach() -endif() - -install(DIRECTORY launch urdf configuration_files - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +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(PROGRAMS scripts/tf_remove_frames.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ + DESTINATION include/ ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +install(DIRECTORY configuration_files urdf launch + DESTINATION share/${PROJECT_NAME}/ ) -# Install source headers. -file(GLOB_RECURSE HDRS "cartographer_ros/*.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() +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 667748dfb..000000000 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ /dev/null @@ -1,166 +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. - -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_node - SRCS - node_main.cc -) - -install(TARGETS cartographer_node - 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} -) - -google_binary(cartographer_pbstream_map_publisher - SRCS - pbstream_map_publisher_main.cc -) - -install(TARGETS cartographer_pbstream_map_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -google_binary(cartographer_dev_pbstream_trajectories_to_rosbag - SRCS - dev/pbstream_trajectories_to_rosbag_main.cc -) - -install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -google_binary(cartographer_dev_rosbag_publisher - SRCS - dev/rosbag_publisher_main.cc -) - -install(TARGETS cartographer_dev_rosbag_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -google_binary(cartographer_dev_trajectory_comparison - SRCS - dev/trajectory_comparison_main.cc -) - -install(TARGETS cartographer_dev_trajectory_comparison - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# TODO(cschuet): Add support for shared library case. -if (${BUILD_GRPC}) - google_binary(cartographer_grpc_node - SRCS - cartographer_grpc/node_grpc_main.cc - ) - - install(TARGETS cartographer_grpc_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - google_binary(cartographer_grpc_offline_node - SRCS - cartographer_grpc/offline_node_grpc_main.cc - ) - - install(TARGETS cartographer_grpc_offline_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - install(PROGRAMS - ../scripts/cartographer_grpc_server.sh - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -endif() diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc deleted file mode 100644 index 48a68c3ec..000000000 --- a/cartographer_ros/cartographer_ros/node.cc +++ /dev/null @@ -1,838 +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 "Eigen/Core" -#include "absl/memory/memory.h" -#include "absl/strings/str_cat.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/StatusCode.h" -#include "cartographer_ros_msgs/StatusResponse.h" -#include "glog/logging.h" -#include "nav_msgs/Odometry.h" -#include "ros/serialization.h" -#include "sensor_msgs/PointCloud2.h" -#include "tf2_eigen/tf2_eigen.h" -#include "visualization_msgs/MarkerArray.h" - -namespace cartographer_ros { - -namespace { - -cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { - cartographer_ros_msgs::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 -::ros::Subscriber SubscribeWithHandler( - void (Node::*handler)(int, const std::string&, - const typename MessageType::ConstPtr&), - const int trajectory_id, const std::string& topic, - ::ros::NodeHandle* const node_handle, Node* const node) { - return node_handle->subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [node, handler, trajectory_id, - topic](const typename MessageType::ConstPtr& msg) { - (node->*handler)(trajectory_id, topic, msg); - })); -} - -} // namespace - -namespace carto = ::cartographer; - -using carto::transform::Rigid3d; -using TrajectoryState = - ::cartographer::mapping::PoseGraphInterface::TrajectoryState; - -Node::Node( - const NodeOptions& node_options, - std::unique_ptr map_builder, - tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) - : node_options_(node_options), - map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { - absl::MutexLock lock(&mutex_); - if (collect_metrics) { - metrics_registry_ = absl::make_unique(); - carto::metrics::RegisterAllMetrics(metrics_registry_.get()); - } - - submap_list_publisher_ = - node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( - kSubmapListTopic, kLatestOnlyPublisherQueueSize); - trajectory_node_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize); - landmark_poses_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize); - constraint_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kConstraintListTopic, kLatestOnlyPublisherQueueSize); - service_servers_.push_back(node_handle_.advertiseService( - kSubmapQueryServiceName, &Node::HandleSubmapQuery, this)); - service_servers_.push_back(node_handle_.advertiseService( - kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); - service_servers_.push_back(node_handle_.advertiseService( - kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); - service_servers_.push_back(node_handle_.advertiseService( - kWriteStateServiceName, &Node::HandleWriteState, this)); - service_servers_.push_back(node_handle_.advertiseService( - kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this)); - service_servers_.push_back(node_handle_.advertiseService( - kReadMetricsServiceName, &Node::HandleReadMetrics, this)); - - scan_matched_point_cloud_publisher_ = - node_handle_.advertise( - kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); - - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.submap_publish_period_sec), - &Node::PublishSubmapList, this)); - if (node_options_.pose_publish_period_sec > 0) { - publish_local_trajectory_data_timer_ = node_handle_.createTimer( - ::ros::Duration(node_options_.pose_publish_period_sec), - &Node::PublishLocalTrajectoryData, this); - } - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), - &Node::PublishTrajectoryNodeList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), - &Node::PublishLandmarkPosesList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(kConstraintPublishPeriodSec), - &Node::PublishConstraintList, this)); -} - -Node::~Node() { FinishAllTrajectories(); } - -::ros::NodeHandle* Node::node_handle() { return &node_handle_; } - -bool Node::HandleSubmapQuery( - ::cartographer_ros_msgs::SubmapQuery::Request& request, - ::cartographer_ros_msgs::SubmapQuery::Response& response) { - absl::MutexLock lock(&mutex_); - map_builder_bridge_.HandleSubmapQuery(request, response); - return true; -} - -void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { - absl::MutexLock lock(&mutex_); - submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); -} - -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(const ::ros::TimerEvent& timer_event) { - 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_.getNumSubscribers() > 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::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(ros::Time::now()), extrapolator.GetLastExtrapolatedTime()); - stamped_transform.header.stamp = - node_options_.use_pose_extrapolator - ? ToRos(now) - : ToRos(trajectory_data.local_slam_data->time); - 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 (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); - } - } - } -} - -void Node::PublishTrajectoryNodeList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { - absl::MutexLock lock(&mutex_); - trajectory_node_list_publisher_.publish( - map_builder_bridge_.GetTrajectoryNodeList()); - } -} - -void Node::PublishLandmarkPosesList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { - absl::MutexLock lock(&mutex_); - landmark_poses_list_publisher_.publish( - map_builder_bridge_.GetLandmarkPosesList()); - } -} - -void Node::PublishConstraintList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (constraint_list_publisher_.getNumSubscribers() > 0) { - absl::MutexLock lock(&mutex_); - constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); - } -} - -std::set -Node::ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::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 Node::AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::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); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(kTopicMismatchCheckDelaySec), - &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); - for (const auto& sensor_id : expected_sensor_ids) { - subscribed_topics_.insert(sensor_id.id); - } - return trajectory_id; -} - -void Node::LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::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( - &Node::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( - &Node::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( - &Node::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(&Node::HandleImuMessage, - trajectory_id, topic, - &node_handle_, this), - topic}); - } - - if (options.use_odometry) { - std::string topic = topics.odometry_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Node::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( - &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, - this), - topic}); - } - if (options.use_landmarks) { - std::string topic = topics.landmark_topic; - subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, - this), - topic}); - } -} - -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 ::cartographer_ros_msgs::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::StatusResponse Node::FinishTrajectoryUnderLock( - const int trajectory_id) { - auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); - - cartographer_ros_msgs::StatusResponse status_response; - if (trajectories_scheduled_for_finish_.count(trajectory_id)) { - const std::string message = absl::StrCat("Trajectory ", trajectory_id, - " already pending to finish."); - status_response.code = cartographer_ros_msgs::StatusCode::OK; - status_response.message = message; - LOG(INFO) << message; - return status_response; - } - - // First, check if we can actually finish the trajectory. - if (!(trajectory_states.count(trajectory_id))) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " is frozen."); - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) { - const std::string error = absl::StrCat("Trajectory ", trajectory_id, - " has already been finished."); - LOG(ERROR) << error; - status_response.code = - cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " has been deleted."); - LOG(ERROR) << error; - status_response.code = - cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; - status_response.message = error; - 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.shutdown(); - 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); - const std::string message = - absl::StrCat("Finished trajectory ", trajectory_id, "."); - status_response.code = cartographer_ros_msgs::StatusCode::OK; - status_response.message = message; - return status_response; -} - -bool Node::HandleStartTrajectory( - ::cartographer_ros_msgs::StartTrajectory::Request& request, - ::cartographer_ros_msgs::StartTrajectory::Response& response) { - absl::MutexLock 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::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::StatusCode::INVALID_ARGUMENT; - response.status.message = error; - } else { - response.trajectory_id = AddTrajectory(options, request.topics); - response.status.code = cartographer_ros_msgs::StatusCode::OK; - response.status.message = "Success."; - } - return true; -} - -void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { - absl::MutexLock lock(&mutex_); - CHECK(ValidateTrajectoryOptions(options)); - AddTrajectory(options, DefaultSensorTopics()); -} - -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), 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 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( - ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, - ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) { - using TrajectoryState = - ::cartographer::mapping::PoseGraphInterface::TrajectoryState; - absl::MutexLock lock(&mutex_); - response.status.code = ::cartographer_ros_msgs::StatusCode::OK; - response.trajectory_states.header.stamp = ros::Time::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::TrajectoryStates::ACTIVE); - break; - case TrajectoryState::FINISHED: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::FINISHED); - break; - case TrajectoryState::FROZEN: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::FROZEN); - break; - case TrajectoryState::DELETED: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::DELETED); - break; - } - } - return true; -} - -bool Node::HandleFinishTrajectory( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response& response) { - absl::MutexLock lock(&mutex_); - response.status = FinishTrajectoryUnderLock(request.trajectory_id); - return true; -} - -bool Node::HandleWriteState( - ::cartographer_ros_msgs::WriteState::Request& request, - ::cartographer_ros_msgs::WriteState::Response& response) { - absl::MutexLock lock(&mutex_); - if (map_builder_bridge_.SerializeState(request.filename, - request.include_unfinished_submaps)) { - response.status.code = cartographer_ros_msgs::StatusCode::OK; - response.status.message = - absl::StrCat("State written to '", request.filename, "'."); - } else { - response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = - absl::StrCat("Failed to write '", request.filename, "'."); - } - return true; -} - -bool Node::HandleReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Request& request, - ::cartographer_ros_msgs::ReadMetrics::Response& response) { - absl::MutexLock lock(&mutex_); - response.timestamp = ros::Time::now(); - if (!metrics_registry_) { - response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE; - response.status.message = "Collection of runtime metrics is not activated."; - return true; - } - metrics_registry_->ReadMetrics(&response); - response.status.code = cartographer_ros_msgs::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::StatusCode::OK); - } - } -} - -bool Node::FinishTrajectory(const int trajectory_id) { - absl::MutexLock lock(&mutex_); - return FinishTrajectoryUnderLock(trajectory_id).code == - cartographer_ros_msgs::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::Odometry::ConstPtr& 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::NavSatFix::ConstPtr& 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::LandmarkList::ConstPtr& 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::Imu::ConstPtr& 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::LaserScan::ConstPtr& 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::MultiEchoLaserScan::ConstPtr& 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::PointCloud2::ConstPtr& 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); -} - -void Node::MaybeWarnAboutTopicMismatch( - const ::ros::WallTimerEvent& unused_timer_event) { - ::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/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc deleted file mode 100644 index eb0e667e6..000000000 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ /dev/null @@ -1,192 +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 "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/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "gflags/gflags.h" -#include "nav_msgs/OccupancyGrid.h" -#include "ros/ros.h" - -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: - 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::SubmapList::ConstPtr& msg); - void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); - - ::ros::NodeHandle node_handle_; - const double resolution_; - - absl::Mutex mutex_; - ::ros::ServiceClient client_ GUARDED_BY(mutex_); - ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); - ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); - std::map submap_slices_ GUARDED_BY(mutex_); - ::ros::WallTimer occupancy_grid_publisher_timer_; - std::string last_frame_id_; - ros::Time last_timestamp_; -}; - -Node::Node(const double resolution, const double publish_period_sec) - : resolution_(resolution), - client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - kSubmapQueryServiceName)), - submap_list_subscriber_(node_handle_.subscribe( - kSubmapListTopic, kLatestOnlyPublisherQueueSize, - boost::function( - [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - HandleSubmapList(msg); - }))), - occupancy_grid_publisher_( - node_handle_.advertise<::nav_msgs::OccupancyGrid>( - FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize, - true /* latched */)), - occupancy_grid_publisher_timer_( - node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), - &Node::DrawAndPublish, this)) {} - -void Node::HandleSubmapList( - const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - absl::MutexLock locker(&mutex_); - - // We do not do any work if nobody listens. - if (occupancy_grid_publisher_.getNumSubscribers() == 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_); - 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; -} - -void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { - 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) { - google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); - - CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps) - << "Ignoring both frozen and unfrozen submaps makes no sense."; - - ::ros::init(argc, argv, "cartographer_occupancy_grid_node"); - ::ros::start(); - - cartographer_ros::ScopedRosLogSink ros_log_sink; - ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec); - - ::ros::spin(); - ::ros::shutdown(); -} 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 b0181d525..000000000 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ /dev/null @@ -1,126 +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 "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_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 = - absl::make_unique( - 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 = - absl::make_unique( - 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 0c092abd0..000000000 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ /dev/null @@ -1,163 +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" - -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) { - TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary); - *options.trajectory_builder_options.mutable_initial_trajectory_pose() = - CreateInitialTrajectoryPose(initial_trajectory_pose); - return options; -} - -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary) { - ::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(ros::Time::now()))); - return pose; -} - -bool FromRosMessage(const cartographer_ros_msgs::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::TrajectoryOptions ToRosMessage( - const TrajectoryOptions& options) { - cartographer_ros_msgs::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/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/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 79% rename from cartographer_ros/cartographer_ros/map_builder_bridge.h rename to cartographer_ros/include/cartographer_ros/map_builder_bridge.h index a9ede0ade..b2c00b751 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/include/cartographer_ros/map_builder_bridge.h @@ -31,17 +31,19 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/trajectory_options.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "nav_msgs/OccupancyGrid.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 "cartographer_ros_msgs/srv/trajectory_query.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" // Abseil unfortunately pulls in winnt.h, which #defines DELETE. -// Clean up to unbreak visualization_msgs::Marker::DELETE. +// Clean up to unbreak visualization_msgs::msg::Marker::DELETE. #ifdef DELETE #undef DELETE #endif -#include "visualization_msgs/MarkerArray.h" +#include "visualization_msgs/msg/marker_array.hpp" namespace cartographer_ros { @@ -82,18 +84,21 @@ class MapBuilderBridge { const bool include_unfinished_submaps); void HandleSubmapQuery( - cartographer_ros_msgs::SubmapQuery::Request& request, - cartographer_ros_msgs::SubmapQuery::Response& response); + 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::SubmapList GetSubmapList(); + cartographer_ros_msgs::msg::SubmapList GetSubmapList(rclcpp::Time node_time); std::unordered_map GetLocalTrajectoryData() LOCKS_EXCLUDED(mutex_); - visualization_msgs::MarkerArray GetTrajectoryNodeList(); - visualization_msgs::MarkerArray GetLandmarkPosesList(); - visualization_msgs::MarkerArray GetConstraintList(); + 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); diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.h b/cartographer_ros/include/cartographer_ros/metrics/family_factory.h similarity index 94% rename from cartographer_ros/cartographer_ros/metrics/family_factory.h rename to cartographer_ros/include/cartographer_ros/metrics/family_factory.h index a05fbd5a7..0c941e5a3 100644 --- a/cartographer_ros/cartographer_ros/metrics/family_factory.h +++ b/cartographer_ros/include/cartographer_ros/metrics/family_factory.h @@ -25,7 +25,7 @@ #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/ReadMetrics.h" +#include "cartographer_ros_msgs/srv/read_metrics.hpp" namespace cartographer_ros { namespace metrics { @@ -47,7 +47,7 @@ class FamilyFactory : public ::cartographer::metrics::FamilyFactory { boundaries) override; void ReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Response* response) const; + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const; private: std::vector> counter_families_; diff --git a/cartographer_ros/cartographer_ros/metrics/internal/counter.h b/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h similarity index 85% rename from cartographer_ros/cartographer_ros/metrics/internal/counter.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/counter.h index 52f35fe88..3ef5666d9 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/counter.h +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h @@ -19,7 +19,7 @@ #include "cartographer/metrics/counter.h" #include "cartographer_ros/metrics/internal/gauge.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -35,9 +35,9 @@ class Counter : public ::cartographer::metrics::Counter { double Value() { return gauge_.Value(); } - cartographer_ros_msgs::Metric ToRosMessage() { - cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage(); - msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER; + 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; } diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.h b/cartographer_ros/include/cartographer_ros/metrics/internal/family.h similarity index 91% rename from cartographer_ros/cartographer_ros/metrics/internal/family.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/family.h index 7e21f58d6..bcab328a1 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/family.h +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/family.h @@ -24,7 +24,7 @@ #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/MetricFamily.h" +#include "cartographer_ros_msgs/msg/metric_family.hpp" namespace cartographer_ros { namespace metrics { @@ -34,7 +34,7 @@ class CounterFamily CounterFamily(const std::string& name, const std::string& description) : name_(name), description_(description) {} Counter* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; @@ -49,7 +49,7 @@ class GaugeFamily : name_(name), description_(description) {} Gauge* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; @@ -66,7 +66,7 @@ class HistogramFamily : public ::cartographer::metrics::Family< Histogram* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; diff --git a/cartographer_ros/cartographer_ros/metrics/internal/gauge.h b/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h similarity index 88% rename from cartographer_ros/cartographer_ros/metrics/internal/gauge.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h index fc0a6935b..f311ab133 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/gauge.h +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h @@ -22,7 +22,7 @@ #include "absl/synchronization/mutex.h" #include "cartographer/metrics/gauge.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -50,11 +50,11 @@ class Gauge : public ::cartographer::metrics::Gauge { return value_; } - cartographer_ros_msgs::Metric ToRosMessage() { - cartographer_ros_msgs::Metric msg; - msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE; + 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::MetricLabel label_msg; + cartographer_ros_msgs::msg::MetricLabel label_msg; label_msg.key = label.first; label_msg.value = label.second; msg.labels.push_back(label_msg); diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.h b/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h similarity index 94% rename from cartographer_ros/cartographer_ros/metrics/internal/histogram.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h index ec688154e..b5d84045c 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/histogram.h +++ b/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h @@ -22,7 +22,7 @@ #include "absl/synchronization/mutex.h" #include "cartographer/metrics/histogram.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -44,7 +44,7 @@ class Histogram : public ::cartographer::metrics::Histogram { double CumulativeCount(); - cartographer_ros_msgs::Metric ToRosMessage(); + cartographer_ros_msgs::msg::Metric ToRosMessage(); private: absl::Mutex mutex_; diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/include/cartographer_ros/msg_conversion.h similarity index 68% rename from cartographer_ros/cartographer_ros/msg_conversion.h rename to cartographer_ros/include/cartographer_ros/msg_conversion.h index ab3fa1e6d..e895144b2 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/include/cartographer_ros/msg_conversion.h @@ -22,56 +22,57 @@ #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros_msgs/LandmarkList.h" -#include "geometry_msgs/Pose.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "nav_msgs/OccupancyGrid.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.h" +#include "cartographer_ros_msgs/msg/landmark_list.hpp" +#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 { -sensor_msgs::PointCloud2 ToPointCloud2Message( +sensor_msgs::msg::PointCloud2 ToPointCloud2Message( int64_t timestamp, const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& point_cloud); -geometry_msgs::Transform ToGeometryMsgTransform( +geometry_msgs::msg::Transform ToGeometryMsgTransform( const ::cartographer::transform::Rigid3d& rigid3d); -geometry_msgs::Pose ToGeometryMsgPose( +geometry_msgs::msg::Pose ToGeometryMsgPose( const ::cartographer::transform::Rigid3d& rigid3d); -geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); // Converts ROS message to point cloud. Returns the time when the last point // was acquired (different from the ROS timestamp). Timing of points is given in // the fourth component of each point relative to `Time`. std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg); ::cartographer::sensor::LandmarkData ToLandmarkData( - const cartographer_ros_msgs::LandmarkList& landmark_list); + const cartographer_ros_msgs::msg::LandmarkList& landmark_list); ::cartographer::transform::Rigid3d ToRigid3d( - const geometry_msgs::TransformStamped& transform); + const geometry_msgs::msg::TransformStamped& transform); -::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); +::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose); -Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3); +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3); -Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion); // Converts from WGS84 (latitude, longitude, altitude) to ECEF. Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, @@ -84,10 +85,10 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, // Points to an occupancy grid message at a specific resolution from painted // submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). -std::unique_ptr CreateOccupancyGridMsg( +std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const ros::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 52% rename from cartographer_ros/cartographer_ros/node.h rename to cartographer_ros/include/cartographer_ros/node.h index 62eb6f927..f3527cae1 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/include/cartographer_ros/node.h @@ -33,25 +33,25 @@ #include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/trajectory_options.h" -#include "cartographer_ros_msgs/FinishTrajectory.h" -#include "cartographer_ros_msgs/GetTrajectoryStates.h" -#include "cartographer_ros_msgs/ReadMetrics.h" -#include "cartographer_ros_msgs/SensorTopics.h" -#include "cartographer_ros_msgs/StartTrajectory.h" -#include "cartographer_ros_msgs/StatusResponse.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" -#include "cartographer_ros_msgs/WriteState.h" -#include "nav_msgs/Odometry.h" -#include "ros/ros.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/NavSatFix.h" -#include "sensor_msgs/PointCloud2.h" -#include "tf2_ros/transform_broadcaster.h" +#include "cartographer_ros_msgs/srv/finish_trajectory.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.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/srv/write_state.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cartographer_ros { @@ -60,7 +60,9 @@ class Node { public: Node(const NodeOptions& node_options, std::unique_ptr map_builder, - tf2_ros::Buffer* tf_buffer, bool collect_metrics); + std::shared_ptr tf_buffer, + rclcpp::Node::SharedPtr node, + bool collect_metrics); ~Node(); Node(const Node&) = delete; @@ -95,21 +97,21 @@ class Node { // The following functions handle adding sensor data to a trajectory. void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::NavSatFix::ConstPtr& msg); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg); void HandleLandmarkMessage( int trajectory_id, const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr &msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::LaserScan::ConstPtr& msg); + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); void HandleMultiEchoLaserScanMessage( int trajectory_id, const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg); void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); // Serializes the complete Node state. void SerializeState(const std::string& filename, @@ -118,11 +120,9 @@ class Node { // Loads a serialized SLAM state from a .pbstream file. void LoadState(const std::string& state_filename, bool load_frozen_state); - ::ros::NodeHandle* node_handle(); - private: struct Subscriber { - ::ros::Subscriber subscriber; + rclcpp::SubscriptionBase::SharedPtr subscriber; // ::ros::Subscriber::getTopic() does not necessarily return the same // std::string @@ -131,65 +131,76 @@ class Node { std::string topic; }; - bool HandleSubmapQuery( - cartographer_ros_msgs::SubmapQuery::Request& request, - cartographer_ros_msgs::SubmapQuery::Response& response); - bool HandleStartTrajectory( - cartographer_ros_msgs::StartTrajectory::Request& request, - cartographer_ros_msgs::StartTrajectory::Response& response); - bool HandleFinishTrajectory( - cartographer_ros_msgs::FinishTrajectory::Request& request, - cartographer_ros_msgs::FinishTrajectory::Response& response); - bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, - cartographer_ros_msgs::WriteState::Response& response); - bool HandleGetTrajectoryStates( - ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, - ::cartographer_ros_msgs::GetTrajectoryStates::Response& response); - bool HandleReadMetrics( - cartographer_ros_msgs::ReadMetrics::Request& request, - cartographer_ros_msgs::ReadMetrics::Response& 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::SensorTopics& topics) const; - int AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics); - void LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics, - int trajectory_id); - void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); + 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 PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event); - void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); - void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); - void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); + void PublishLocalTrajectoryData(); + void PublishTrajectoryNodeList(); + void PublishLandmarkPosesList(); + void PublishConstraintList(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); - bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options); - cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( + bool ValidateTopicNames(const TrajectoryOptions& options); + cartographer_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock( int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); - void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); + 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_; - tf2_ros::TransformBroadcaster tf_broadcaster_; + std::shared_ptr tf_broadcaster_; absl::Mutex mutex_; std::unique_ptr metrics_registry_; - MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); + std::shared_ptr map_builder_bridge_ GUARDED_BY(mutex_); + + 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_; + ::rclcpp::Service::SharedPtr get_trajectory_states_server_; + ::rclcpp::Service::SharedPtr read_metrics_server_; - ::ros::NodeHandle node_handle_; - ::ros::Publisher submap_list_publisher_; - ::ros::Publisher trajectory_node_list_publisher_; - ::ros::Publisher landmark_poses_list_publisher_; - ::ros::Publisher constraint_list_publisher_; - // These ros::ServiceServers need to live for the lifetime of the node. - std::vector<::ros::ServiceServer> service_servers_; - ::ros::Publisher scan_matched_point_cloud_publisher_; struct TrajectorySensorSamplers { TrajectorySensorSamplers(const double rangefinder_sampling_ratio, @@ -212,20 +223,22 @@ class 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_set trajectories_scheduled_for_finish_; - // We have to keep the timer handles of ::ros::WallTimers around, otherwise - // they do not fire. - std::vector<::ros::WallTimer> 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. - ::ros::Timer publish_local_trajectory_data_timer_; + ::rclcpp::TimerBase::SharedPtr submap_list_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 95% rename from cartographer_ros/cartographer_ros/node_constants.h rename to cartographer_ros/include/cartographer_ros/node_constants.h index b819121a8..07a85f379 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/include/cartographer_ros/node_constants.h @@ -34,7 +34,9 @@ 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"; diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/include/cartographer_ros/node_options.h similarity index 96% rename from cartographer_ros/cartographer_ros/node_options.h rename to cartographer_ros/include/cartographer_ros/node_options.h index 30215812a..95bb15990 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/include/cartographer_ros/node_options.h @@ -35,6 +35,8 @@ 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; }; @@ -44,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 68% rename from cartographer_ros/cartographer_ros/playable_bag.h rename to cartographer_ros/include/cartographer_ros/playable_bag.h index 06c608628..6cdf2df60 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/include/cartographer_ros/playable_bag.h @@ -16,13 +16,16 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H + #include #include -#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 { @@ -33,61 +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( - cartographer_ros_msgs::BagfileProgress* progress); + 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_; + double duration_in_seconds_; int message_counter_; - std::deque buffered_messages_; - const ::ros::Duration buffer_delay_; + std::deque buffered_messages_; + const rclcpp::Duration buffer_delay_; FilteringEarlyMessageHandler filtering_early_message_handler_; std::set topics_; }; class PlayableBagMultiplexer { public: - PlayableBagMultiplexer(); + 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) { @@ -96,11 +96,11 @@ class PlayableBagMultiplexer { }; }; - ros::NodeHandle pnh_; + rclcpp::Node::SharedPtr node_; // Publishes information about the bag-file(s) processing and its progress - ros::Publisher bag_progress_pub_; + 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_; + std::map bag_progress_time_map_; // The time interval of publishing bag-file(s) processing in seconds double progress_pub_interval_; 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 96% 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 71c165b42..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,7 @@ #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 { diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/include/cartographer_ros/sensor_bridge.h similarity index 73% rename from cartographer_ros/cartographer_ros/sensor_bridge.h rename to cartographer_ros/include/cartographer_ros/sensor_bridge.h index 807d81648..e53a0c9c4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/include/cartographer_ros/sensor_bridge.h @@ -26,16 +26,16 @@ #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros_msgs/LandmarkList.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "nav_msgs/OccupancyGrid.h" -#include "nav_msgs/Odometry.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/NavSatFix.h" -#include "sensor_msgs/PointCloud2.h" +#include "cartographer_ros_msgs/msg/landmark_list.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cartographer_ros { @@ -51,26 +51,26 @@ class SensorBridge { SensorBridge& operator=(const SensorBridge&) = delete; std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData( - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleOdometryMessage(const std::string& sensor_id, - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleNavSatFixMessage(const std::string& sensor_id, - const sensor_msgs::NavSatFix::ConstPtr& msg); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg); void HandleLandmarkMessage( const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg); std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr& msg); void HandleImuMessage(const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr& msg); void HandleLaserScanMessage(const std::string& sensor_id, - const sensor_msgs::LaserScan::ConstPtr& msg); + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); void HandleMultiEchoLaserScanMessage( const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg); void HandlePointCloud2Message(const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); const TfBridge& tf_bridge() const; 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 0df22154c..48c3a425b 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/include/cartographer_ros/submap.h @@ -25,7 +25,8 @@ #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 namespace cartographer_ros { @@ -33,7 +34,9 @@ namespace cartographer_ros { // on error. std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, - ros::ServiceClient* client); + 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 82% rename from cartographer_ros/cartographer_ros/time_conversion.h rename to cartographer_ros/include/cartographer_ros/time_conversion.h index dda5bb5ab..5555f4d49 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.h +++ b/cartographer_ros/include/cartographer_ros/time_conversion.h @@ -18,13 +18,14 @@ #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H #include "cartographer/common/time.h" -#include "ros/ros.h" +#include +#include namespace cartographer_ros { -::ros::Time ToRos(::cartographer::common::Time time); +rclcpp::Time ToRos(::cartographer::common::Time time); -::cartographer::common::Time FromRos(const ::ros::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 70% rename from cartographer_ros/cartographer_ros/trajectory_options.h rename to cartographer_ros/include/cartographer_ros/trajectory_options.h index 97be4c97f..93817b7d0 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/include/cartographer_ros/trajectory_options.h @@ -22,7 +22,6 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" namespace cartographer_ros { @@ -48,25 +47,9 @@ struct TrajectoryOptions { double landmarks_sampling_ratio; }; -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); - TrajectoryOptions CreateTrajectoryOptions( ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose); - -// Try to convert 'msg' into 'options'. Returns false on failure. -bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, - TrajectoryOptions* options); - -// Converts 'trajectory_options' into a ROS message. -cartographer_ros_msgs::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/offline_backpack_2d.launch b/cartographer_ros/launch/offline_backpack_2d.launch deleted file mode 100644 index bb8ae0a70..000000000 --- a/cartographer_ros/launch/offline_backpack_2d.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - 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 8dfa0f836..000000000 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ /dev/null @@ -1,37 +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 b/cartographer_ros/launch/offline_node.launch deleted file mode 100644 index cf404a007..000000000 --- a/cartographer_ros/launch/offline_node.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - 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 cf5b7e034..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 69026693b..1ce505e0c 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -15,9 +15,9 @@ limitations under the License. --> - + cartographer_ros - 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,34 +28,34 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors - catkin + ament_cmake git google-mock protobuf-dev - python-sphinx + python3-sphinx - cartographer - cartographer_ros_msgs - eigen_conversions + cartographer + cartographer_ros_msgs geometry_msgs libgflags-dev libgoogle-glog-dev libpcl-all-dev - message_runtime + builtin_interfaces + rosidl_default_generators nav_msgs pcl_conversions robot_state_publisher - rosbag - roscpp - roslaunch - roslib + rosbag2 + rosbag2_transport + rclcpp + launch sensor_msgs std_msgs tf2 @@ -64,9 +64,7 @@ urdf visualization_msgs - rosunit - - + ament_cmake diff --git a/cartographer_ros/cartographer_ros/.clang-format b/cartographer_ros/src/.clang-format similarity index 100% rename from cartographer_ros/cartographer_ros/.clang-format rename to cartographer_ros/src/.clang-format diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/src/assets_writer.cpp similarity index 66% rename from cartographer_ros/cartographer_ros/assets_writer.cc rename to cartographer_ros/src/assets_writer.cpp index 1ca3e099f..5f75b4135 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/src/assets_writer.cpp @@ -39,12 +39,11 @@ #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 "builtin_interfaces/msg/time.hpp" +#include +#include #include "tf2_eigen/tf2_eigen.h" -#include "tf2_msgs/TFMessage.h" +#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/buffer.h" #include "urdf/model.h" @@ -92,19 +91,19 @@ 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 = 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) { @@ -117,8 +116,8 @@ std::unique_ptr HandleMessage( 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( @@ -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); + delayed_messages.push_back(*message); LOG_EVERY_N(INFO, 10000) - << "Processed " << (message.getTime() - begin_time).toSec() - << " of " << duration_in_seconds << " bag time seconds..."; + << "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); 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 238f0df7b..eaa8422f5 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/src/assets_writer_main.cpp @@ -14,10 +14,11 @@ * limitations under the License. */ -#include "absl/strings/str_split.h" #include "cartographer_ros/assets_writer.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, - absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()), + 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 100% rename from cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc rename to cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp 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 100% 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 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/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc b/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp similarity index 95% rename from cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc rename to cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp index 63d2c5f24..27da714ff 100644 --- a/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc +++ b/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp @@ -35,12 +35,12 @@ DEFINE_string(parent_frame, "map", "Frame id to use as parent frame."); namespace cartographer_ros { namespace { -geometry_msgs::TransformStamped ToTransformStamped( +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::TransformStamped transform_stamped; + 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( @@ -67,7 +67,7 @@ void pbstream_trajectories_to_bag(const std::string& pbstream_filename, << " nodes."; for (const auto& node : trajectory.node()) { tf2_msgs::TFMessage tf_msg; - geometry_msgs::TransformStamped transform_stamped = ToTransformStamped( + 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, 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 100% rename from cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc rename to cartographer_ros/src/dev/trajectory_comparison_main.cpp diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/src/map_builder_bridge.cpp similarity index 79% rename from cartographer_ros/cartographer_ros/map_builder_bridge.cc rename to cartographer_ros/src/map_builder_bridge.cpp index ea51b9abf..4ea2b191d 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/src/map_builder_bridge.cpp @@ -17,13 +17,12 @@ #include "cartographer_ros/map_builder_bridge.h" #include "absl/memory/memory.h" -#include "absl/strings/str_cat.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_msgs/StatusCode.h" -#include "cartographer_ros_msgs/StatusResponse.h" +#include "cartographer_ros/time_conversion.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/msg/status_response.hpp" namespace cartographer_ros { namespace { @@ -34,8 +33,8 @@ constexpr double kTrajectoryLineStripMarkerScale = 0.07; constexpr double kLandmarkMarkerScale = 0.2; constexpr double kConstraintMarkerScale = 0.025; -::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { - ::std_msgs::ColorRGBA result; +::std_msgs::msg::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { + ::std_msgs::msg::ColorRGBA result; result.r = color[0]; result.g = color[1]; result.b = color[2]; @@ -43,13 +42,14 @@ ::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { return result; } -visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id, - const std::string& frame_id) { - visualization_msgs::Marker marker; - marker.ns = absl::StrCat("Trajectory ", trajectory_id); +visualization_msgs::msg::Marker CreateTrajectoryMarker(const int trajectory_id, + 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; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.header.stamp = ::ros::Time::now(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.header.stamp = node_time; marker.header.frame_id = frame_id; marker.color = ToMessage(cartographer::io::GetColor(trajectory_id)); marker.scale.x = kTrajectoryLineStripMarkerScale; @@ -70,14 +70,15 @@ int GetLandmarkIndex( return it->second; } -visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, +visualization_msgs::msg::Marker CreateLandmarkMarker(int landmark_index, const Rigid3d& landmark_pose, - const std::string& frame_id) { - visualization_msgs::Marker marker; + const std::string& frame_id, + rclcpp::Time node_time) { + visualization_msgs::msg::Marker marker; marker.ns = "Landmarks"; marker.id = landmark_index; - marker.type = visualization_msgs::Marker::SPHERE; - marker.header.stamp = ::ros::Time::now(); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.header.stamp = node_time; marker.header.frame_id = frame_id; marker.scale.x = kLandmarkMarkerScale; marker.scale.y = kLandmarkMarkerScale; @@ -87,8 +88,8 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, return marker; } -void PushAndResetLineMarker(visualization_msgs::Marker* marker, - std::vector* markers) { +void PushAndResetLineMarker(visualization_msgs::msg::Marker* marker, + std::vector* markers) { markers->push_back(*marker); ++marker->id; marker->points.clear(); @@ -168,24 +169,24 @@ bool MapBuilderBridge::SerializeState(const std::string& filename, } void MapBuilderBridge::HandleSubmapQuery( - cartographer_ros_msgs::SubmapQuery::Request& request, - cartographer_ros_msgs::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}; + cartographer::mapping::SubmapId submap_id{request->trajectory_id, + request->submap_index}; const std::string error = map_builder_->SubmapToProto(submap_id, &response_proto); if (!error.empty()) { LOG(ERROR) << error; - response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; - response.status.message = error; + response->status.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND; + response->status.message = error; return; } - response.submap_version = response_proto.submap_version(); + response->submap_version = response_proto.submap_version(); for (const auto& texture_proto : response_proto.textures()) { - response.textures.emplace_back(); - auto& texture = response.textures.back(); + response->textures.emplace_back(); + auto& texture = response->textures.back(); texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(), texture_proto.cells().end()); texture.width = texture_proto.width(); @@ -194,8 +195,8 @@ void MapBuilderBridge::HandleSubmapQuery( texture.slice_pose = ToGeometryMsgPose( cartographer::transform::ToRigid3(texture_proto.slice_pose())); } - response.status.message = "Success."; - response.status.code = cartographer_ros_msgs::StatusCode::OK; + response->status.message = "Success."; + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; } std::map @@ -206,18 +207,18 @@ MapBuilderBridge::GetTrajectoryStates() { for (const auto& sensor_bridge : sensor_bridges_) { trajectory_states.insert(std::make_pair( sensor_bridge.first, - ::cartographer::mapping::PoseGraph::TrajectoryState::ACTIVE)); + ::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE)); } return trajectory_states; } -cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { - cartographer_ros_msgs::SubmapList submap_list; - submap_list.header.stamp = ::ros::Time::now(); +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::SubmapEntry submap_entry; + 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; @@ -258,8 +259,34 @@ MapBuilderBridge::GetLocalTrajectoryData() { return local_trajectory_data; } -visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { - visualization_msgs::MarkerArray trajectory_node_list; +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 trajectory_node_list; const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); // Find the last node indices for each trajectory that have either // inter-submap or inter-trajectory constraints. @@ -274,7 +301,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { 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 @@ -293,8 +320,8 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { } for (const int trajectory_id : node_poses.trajectory_ids()) { - visualization_msgs::Marker marker = - CreateTrajectoryMarker(trajectory_id, node_options_.map_frame); + visualization_msgs::msg::Marker marker = + CreateTrajectoryMarker(trajectory_id, node_options_.map_frame, node_time); int last_inter_submap_constrained_node = std::max( node_poses.trajectory(trajectory_id).begin()->id.node_index, trajectory_to_last_inter_submap_constrained_node.at(trajectory_id)); @@ -318,7 +345,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { PushAndResetLineMarker(&marker, &trajectory_node_list.markers); continue; } - const ::geometry_msgs::Point node_point = + const ::geometry_msgs::msg::Point node_point = ToGeometryMsgPoint(node_id_data.data.global_pose.translation()); marker.points.push_back(node_point); @@ -346,7 +373,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) { trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id; } else { - marker.action = visualization_msgs::Marker::DELETE; + marker.action = visualization_msgs::msg::Marker::DELETE; while (static_cast(marker.id) <= trajectory_to_highest_marker_id_[trajectory_id]) { trajectory_node_list.markers.push_back(marker); @@ -358,31 +385,33 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { return trajectory_node_list; } -visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() { - visualization_msgs::MarkerArray landmark_poses_list; +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(); for (const auto& id_to_pose : landmark_poses) { landmark_poses_list.markers.push_back(CreateLandmarkMarker( GetLandmarkIndex(id_to_pose.first, &landmark_to_index_), - id_to_pose.second, node_options_.map_frame)); + id_to_pose.second, node_options_.map_frame, node_time)); } return landmark_poses_list; } -visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { - visualization_msgs::MarkerArray constraint_list; +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(rclcpp::Time node_time) { + visualization_msgs::msg::MarkerArray constraint_list; int marker_id = 0; - visualization_msgs::Marker constraint_intra_marker; + visualization_msgs::msg::Marker constraint_intra_marker; constraint_intra_marker.id = marker_id++; constraint_intra_marker.ns = "Intra constraints"; - constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST; - constraint_intra_marker.header.stamp = ros::Time::now(); + constraint_intra_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + constraint_intra_marker.header.stamp = node_time; constraint_intra_marker.header.frame_id = node_options_.map_frame; constraint_intra_marker.scale.x = kConstraintMarkerScale; constraint_intra_marker.pose.orientation.w = 1.0; - visualization_msgs::Marker residual_intra_marker = constraint_intra_marker; + visualization_msgs::msg::Marker residual_intra_marker = constraint_intra_marker; residual_intra_marker.id = marker_id++; residual_intra_marker.ns = "Intra residuals"; // This and other markers which are less numerous are set to be slightly @@ -390,27 +419,27 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { // visible. residual_intra_marker.pose.position.z = 0.1; - visualization_msgs::Marker constraint_inter_same_trajectory_marker = + visualization_msgs::msg::Marker constraint_inter_same_trajectory_marker = constraint_intra_marker; constraint_inter_same_trajectory_marker.id = marker_id++; constraint_inter_same_trajectory_marker.ns = "Inter constraints, same trajectory"; constraint_inter_same_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker residual_inter_same_trajectory_marker = + visualization_msgs::msg::Marker residual_inter_same_trajectory_marker = constraint_intra_marker; residual_inter_same_trajectory_marker.id = marker_id++; residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory"; residual_inter_same_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker constraint_inter_diff_trajectory_marker = + visualization_msgs::msg::Marker constraint_inter_diff_trajectory_marker = constraint_intra_marker; constraint_inter_diff_trajectory_marker.id = marker_id++; constraint_inter_diff_trajectory_marker.ns = "Inter constraints, different trajectories"; constraint_inter_diff_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker residual_inter_diff_trajectory_marker = + visualization_msgs::msg::Marker residual_inter_diff_trajectory_marker = constraint_intra_marker; residual_inter_diff_trajectory_marker.id = marker_id++; residual_inter_diff_trajectory_marker.ns = @@ -423,10 +452,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { const auto constraints = map_builder_->pose_graph()->constraints(); for (const auto& constraint : constraints) { - visualization_msgs::Marker *constraint_marker, *residual_marker; - std_msgs::ColorRGBA color_constraint, color_residual; + 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 diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.cc b/cartographer_ros/src/metrics/family_factory.cpp similarity index 96% rename from cartographer_ros/cartographer_ros/metrics/family_factory.cc rename to cartographer_ros/src/metrics/family_factory.cpp index 96df38179..388c226f5 100644 --- a/cartographer_ros/cartographer_ros/metrics/family_factory.cc +++ b/cartographer_ros/src/metrics/family_factory.cpp @@ -53,7 +53,7 @@ FamilyFactory::NewHistogramFamily(const std::string& name, } void FamilyFactory::ReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Response* response) const { + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const { for (const auto& counter_family : counter_families_) { response->metric_families.push_back(counter_family->ToRosMessage()); } diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.cc b/cartographer_ros/src/metrics/internal/family.cpp similarity index 85% rename from cartographer_ros/cartographer_ros/metrics/internal/family.cc rename to cartographer_ros/src/metrics/internal/family.cpp index 42f81c65f..29572a9e8 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/family.cc +++ b/cartographer_ros/src/metrics/internal/family.cpp @@ -33,8 +33,8 @@ Counter* CounterFamily::Add(const std::map& labels) { return ptr; } -cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +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_) { @@ -50,8 +50,8 @@ Gauge* GaugeFamily::Add(const std::map& labels) { return ptr; } -cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +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_) { @@ -68,8 +68,8 @@ Histogram* HistogramFamily::Add( return ptr; } -cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +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_) { diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc b/cartographer_ros/src/metrics/internal/histogram.cpp similarity index 90% rename from cartographer_ros/cartographer_ros/metrics/internal/histogram.cc rename to cartographer_ros/src/metrics/internal/histogram.cpp index 27646ccd0..40fe625a4 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc +++ b/cartographer_ros/src/metrics/internal/histogram.cpp @@ -68,17 +68,17 @@ double Histogram::CumulativeCount() { return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.); } -cartographer_ros_msgs::Metric Histogram::ToRosMessage() { - cartographer_ros_msgs::Metric msg; - msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM; +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::MetricLabel label_msg; + 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::HistogramBucket bucket_msg; + 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); diff --git a/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc b/cartographer_ros/src/metrics/internal/metrics_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc rename to cartographer_ros/src/metrics/internal/metrics_test.cpp diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cpp similarity index 83% rename from cartographer_ros/cartographer_ros/msg_conversion.cc rename to cartographer_ros/src/msg_conversion.cpp index 7e19088e2..973563ee6 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/src/msg_conversion.cpp @@ -25,22 +25,22 @@ #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/time_conversion.h" -#include "geometry_msgs/Pose.h" -#include "geometry_msgs/Quaternion.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "geometry_msgs/Vector3.h" +#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 "nav_msgs/OccupancyGrid.h" +#include "nav_msgs/msg/occupancy_grid.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" -#include "ros/ros.h" -#include "ros/serialization.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.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" namespace { @@ -73,7 +73,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( 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.; @@ -82,13 +82,13 @@ using ::cartographer::sensor::LandmarkData; using ::cartographer::sensor::LandmarkObservation; using ::cartographer::sensor::PointCloudWithIntensities; using ::cartographer::transform::Rigid3d; -using ::cartographer_ros_msgs::LandmarkEntry; -using ::cartographer_ros_msgs::LandmarkList; +using ::cartographer_ros_msgs::msg::LandmarkEntry; +using ::cartographer_ros_msgs::msg::LandmarkList; -sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, +sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, const std::string& frame_id, const int num_points) { - sensor_msgs::PointCloud2 msg; + sensor_msgs::msg::PointCloud2 msg; msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); msg.header.frame_id = frame_id; msg.height = 1; @@ -96,15 +96,15 @@ sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, msg.fields.resize(3); msg.fields[0].name = "x"; msg.fields[0].offset = 0; - msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[0].count = 1; msg.fields[1].name = "y"; msg.fields[1].offset = 4; - msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[1].count = 1; msg.fields[2].name = "z"; msg.fields[2].offset = 8; - msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[2].count = 1; msg.is_bigendian = false; msg.point_step = 16; @@ -114,21 +114,21 @@ sensor_msgs::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. -bool HasEcho(const sensor_msgs::LaserEcho& echo) { +// For sensor_msgs::msg::MultiEchoLaserScan. +bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) { return !echo.echoes.empty(); } -float GetFirstEcho(const sensor_msgs::LaserEcho& echo) { +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) { @@ -174,7 +174,7 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { return std::make_tuple(point_cloud, timestamp); } -bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, +bool PointCloud2HasField(const sensor_msgs::msg::PointCloud2& pc2, const std::string& field_name) { for (const auto& field : pc2.fields) { if (field.name == field_name) { @@ -186,35 +186,36 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, } // namespace -sensor_msgs::PointCloud2 ToPointCloud2Message( +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()); - ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); - for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) { - stream.next(point.position.x()); - stream.next(point.position.y()); - stream.next(point.position.z()); - stream.next(kPointCloudComponentFourMagic); + size_t offset = 0; + float * const data = reinterpret_cast(&msg.data[0]); + for (const auto& point : point_cloud) { + data[offset++] = point.position.x(); + data[offset++] = point.position.y(); + data[offset++] = point.position.z(); + data[offset++] = kPointCloudComponentFourMagic; } return msg; } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) { +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) { +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { +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. @@ -289,27 +290,27 @@ LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { return landmark_data; } -Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { +Rigid3d ToRigid3d(const geometry_msgs::msg::TransformStamped& transform) { return Rigid3d(ToEigen(transform.transform.translation), ToEigen(transform.transform.rotation)); } -Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { +Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose) { return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, ToEigen(pose.orientation)); } -Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) { +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3) { return Eigen::Vector3d(vector3.x, vector3.y, vector3.z); } -Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) { +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) { return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y, quaternion.z); } -geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { - geometry_msgs::Transform transform; +geometry_msgs::msg::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { + geometry_msgs::msg::Transform transform; transform.translation.x = rigid3d.translation().x(); transform.translation.y = rigid3d.translation().y(); transform.translation.z = rigid3d.translation().z(); @@ -320,8 +321,8 @@ geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { return transform; } -geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { - geometry_msgs::Pose pose; +geometry_msgs::msg::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { + geometry_msgs::msg::Pose pose; pose.position = ToGeometryMsgPoint(rigid3d.translation()); pose.orientation.w = rigid3d.rotation().w(); pose.orientation.x = rigid3d.rotation().x(); @@ -330,8 +331,8 @@ geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { return pose; } -geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { - geometry_msgs::Point point; +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { + geometry_msgs::msg::Point point; point.x = vector3d.x(); point.y = vector3d.y(); point.z = vector3d.z(); @@ -370,11 +371,11 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( return cartographer::transform::Rigid3d(rotation * -translation, rotation); } -std::unique_ptr CreateOccupancyGridMsg( +std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const ros::Time& time) { - auto occupancy_grid = absl::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 = diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/src/msg_conversion_test.cpp similarity index 97% rename from cartographer_ros/cartographer_ros/msg_conversion_test.cc rename to cartographer_ros/src/msg_conversion_test.cpp index 89c8246d3..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); } @@ -72,7 +73,7 @@ TEST(MsgConversion, LaserScanToPointCloud) { } 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); @@ -109,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; diff --git a/cartographer_ros/src/node.cpp b/cartographer_ros/src/node.cpp new file mode 100644 index 000000000..2683501ba --- /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.h" +#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 70% rename from cartographer_ros/cartographer_ros/node_main.cc rename to cartographer_ros/src/node_main.cpp index e430af24d..f403be079 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/src/node_main.cpp @@ -48,33 +48,43 @@ namespace cartographer_ros { namespace { void Run() { + rclcpp::Node::SharedPtr cartographer_node = rclcpp::Node::make_shared("cartographer_node"); constexpr double kTfBufferCacheTimeInSeconds = 10.; - tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; - tf2_ros::TransformListener tf(tf_buffer); + + 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 = absl::make_unique( - node_options.map_builder_options); - Node node(node_options, std::move(map_builder), &tf_buffer, - FLAGS_collect_metrics); + auto 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); + node->LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { - node.StartTrajectoryWithDefaultTopics(trajectory_options); + node->StartTrajectoryWithDefaultTopics(trajectory_options); } - ::ros::spin(); + rclcpp::spin(cartographer_node); - node.FinishAllTrajectories(); - node.RunFinalOptimization(); + 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 */); } } @@ -83,18 +93,19 @@ void Run() { } // 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, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing."; CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; - ::ros::init(argc, argv, "cartographer_node"); - ::ros::start(); - cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::Run(); - ::ros::shutdown(); + ::rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/src/node_options.cpp similarity index 86% rename from cartographer_ros/cartographer_ros/node_options.cc rename to cartographer_ros/src/node_options.cpp index 4812d08f0..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,6 +40,14 @@ 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"); 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/cartographer_ros/offline_node.cc b/cartographer_ros/src/offline_node.cpp similarity index 53% rename from cartographer_ros/cartographer_ros/offline_node.cc rename to cartographer_ros/src/offline_node.cpp index b2cf20586..3cfa9dcd5 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/src/offline_node.cpp @@ -17,22 +17,24 @@ #include "cartographer_ros/offline_node.h" #include -#include +#include #ifndef WIN32 #include #endif #include + #include -#include "absl/strings/str_split.h" #include "cartographer_ros/node.h" #include "cartographer_ros/playable_bag.h" #include "cartographer_ros/urdf_reader.h" #include "gflags/gflags.h" -#include "ros/callback_queue.h" -#include "rosgraph_msgs/Clock.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 " @@ -62,6 +64,11 @@ DEFINE_string(load_state_filename, "", "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."); @@ -73,26 +80,42 @@ namespace cartographer_ros { constexpr char kClockTopic[] = "clock"; constexpr char kTfStaticTopic[] = "/tf_static"; -constexpr char kTfTopic[] = "tf"; +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); +const rclcpp::Duration kDelay(1.0, 0); -void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { +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."; - const std::vector bag_filenames = - absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()); + 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; - const std::vector configuration_basenames = - absl::StrSplit(FLAGS_configuration_basenames, ',', absl::SkipEmpty()); + 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)); @@ -121,50 +144,51 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { const std::chrono::time_point start_time = std::chrono::steady_clock::now(); - tf2_ros::Buffer tf_buffer; - - std::vector urdf_transforms; - const std::vector urdf_filenames = - absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty()); - 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()); + 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); - tf_buffer.setUsingDedicatedThread(true); - - Node node(node_options, std::move(map_builder), &tf_buffer, + 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); } - ::ros::Publisher tf_publisher = - node.node_handle()->advertise( + rclcpp::Publisher::SharedPtr tf_publisher = + cartographer_offline_node->create_publisher( kTfTopic, kLatestOnlyPublisherQueueSize); - ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; + ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster(cartographer_offline_node); - ::ros::Publisher clock_publisher = - node.node_handle()->advertise( + rclcpp::Publisher::SharedPtr clock_publisher = + cartographer_offline_node->create_publisher( 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 */); + rosgraph_msgs::msg::Clock clock; std::vector< std::set> @@ -184,20 +208,21 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { std::map, cartographer::mapping::TrajectoryBuilderInterface::SensorId> bag_topic_to_sensor_id; - PlayableBagMultiplexer playable_bag_multiplexer; + 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 (!::ros::ok()) { + 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), - node.node_handle()->resolveName(expected_sensor_id.id)); + "/"+expected_sensor_id.id); if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) { - LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag " + 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 " @@ -206,30 +231,37 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { 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, ros::TIME_MIN, ros::TIME_MAX, kDelay, + 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](const rosbag::MessageInstance& msg) { - if (msg.isType()) { + [&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) { - 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(); + 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 @@ -244,7 +276,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { std::set bag_topics; std::stringstream bag_topics_string; for (const auto& topic : playable_bag_multiplexer.topics()) { - std::string resolved_topic = node.node_handle()->resolveName(topic, false); + 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 << ","; } @@ -263,22 +296,33 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { } std::unordered_map bag_index_to_trajectory_id; - const ros::Time begin_time = + 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() - : ros::Time(); + : 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 (!::ros::ok()) { + if (!::rclcpp::ok()) { return; } const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); - const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); + const rosbag2_storage::SerializedBagMessage& 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); + 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.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) { + if (msg.time_stamp < + (begin_time.nanoseconds() + rclcpp::Duration(FLAGS_skip_seconds, 0).nanoseconds())) { continue; } @@ -302,44 +346,67 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { const auto bag_topic = std::make_pair( bag_index, - node.node_handle()->resolveName(msg.getTopic(), false /* resolve */)); + 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 (msg.isType()) { + + 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, - 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()) { + 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, - msg.instantiate()); - } - if (msg.isType()) { + 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, - msg.instantiate()); - } - if (msg.isType()) { + 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, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleLandmarkMessage( - trajectory_id, sensor_id, - msg.instantiate()); + 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 = msg.getTime(); - clock_publisher.publish(clock); + 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); @@ -349,7 +416,12 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { // 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(); + // 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 = @@ -370,16 +442,20 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { 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; + // 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) { - ::ros::waitForShutdown(); + LOG(INFO) << "Finished processing and waiting for shutdown."; + rclcpp::spin(cartographer_offline_node); } } 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 ca593bb48..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 absl::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 78% rename from cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc rename to cartographer_ros/src/pbstream_map_publisher_main.cpp index 3ca601fd5..8d149be65 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/src/pbstream_map_publisher_main.cpp @@ -31,8 +31,8 @@ #include "cartographer_ros/ros_map.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."); @@ -43,7 +43,7 @@ DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); namespace cartographer_ros { namespace { -std::unique_ptr LoadOccupancyGridMsg( +std::unique_ptr LoadOccupancyGridMsg( const std::string& pbstream_filename, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); @@ -60,40 +60,42 @@ std::unique_ptr LoadOccupancyGridMsg( const auto painted_slices = ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id, - ros::Time::now()); + rclcpp::Clock().now()); } void Run(const std::string& pbstream_filename, const std::string& map_topic, const std::string& map_frame_id, const double resolution) { - std::unique_ptr msg_ptr = + 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); - ::ros::NodeHandle node_handle(""); - ::ros::Publisher pub = node_handle.advertise( - map_topic, kLatestOnlyPublisherQueueSize, true /*latched */); + 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) { + // 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 92% rename from cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc rename to cartographer_ros/src/pbstream_to_ros_map_main.cpp index e92f0a0d2..1ef4539a7 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/src/pbstream_to_ros_map_main.cpp @@ -26,6 +26,7 @@ #include "cartographer_ros/ros_map.h" #include "gflags/gflags.h" #include "glog/logging.h" +#include DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from."); @@ -69,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/cartographer_ros/playable_bag.cc b/cartographer_ros/src/playable_bag.cpp similarity index 51% rename from cartographer_ros/cartographer_ros/playable_bag.cc rename to cartographer_ros/src/playable_bag.cpp index f305ad850..f9ef8fcd4 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.cc +++ b/cartographer_ros/src/playable_bag.cpp @@ -19,49 +19,58 @@ #include "absl/memory/memory.h" #include "cartographer_ros/node_constants.h" #include "glog/logging.h" -#include "tf2_msgs/TFMessage.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 ros::Time start_time, const ros::Time end_time, - const ros::Duration buffer_delay, + const rclcpp::Duration buffer_delay, FilteringEarlyMessageHandler filtering_early_message_handler) - : bag_(absl::make_unique(bag_filename, rosbag::bagmode::Read)), - view_(absl::make_unique(*bag_, start_time, end_time)), - view_iterator_(view_->begin()), + : bag_reader_(std::make_unique()), finished_(false), bag_id_(bag_id), bag_filename_(bag_filename), - duration_in_seconds_( - (view_->getEndTime() - view_->getBeginTime()).toSec()), 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 (const auto* connection_info : view_->getConnections()) { - topics_.insert(connection_info->topic); + for (auto topic_info : bag_metadata.topics_with_message_count) { + topics_.insert(topic_info.topic_metadata.name); } } -ros::Time PlayableBag::PeekMessageTime() const { +rclcpp::Time PlayableBag::PeekMessageTime() const { CHECK(IsMessageAvailable()); - return buffered_messages_.front().getTime(); + return rclcpp::Time(buffered_messages_.front().time_stamp); } -std::tuple PlayableBag::GetBeginEndTime() const { - return std::make_tuple(view_->getBeginTime(), view_->getEndTime()); +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())); } -rosbag::MessageInstance PlayableBag::GetNextMessage( - cartographer_ros_msgs::BagfileProgress* progress) { +rosbag2_storage::SerializedBagMessage PlayableBag::GetNextMessage( + cartographer_ros_msgs::msg::BagfileProgress* progress) { CHECK(IsMessageAvailable()); - const rosbag::MessageInstance msg = buffered_messages_.front(); + const rosbag2_storage::SerializedBagMessage msg = buffered_messages_.front(); buffered_messages_.pop_front(); AdvanceUntilMessageAvailable(); - double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec(); + 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_; @@ -70,7 +79,7 @@ rosbag::MessageInstance PlayableBag::GetNextMessage( if (progress) { progress->current_bagfile_name = bag_filename_; progress->current_bagfile_id = bag_id_; - progress->total_messages = view_->size(); + progress->total_messages = bag_metadata.message_count; progress->processed_messages = message_counter_; progress->total_seconds = duration_in_seconds_; progress->processed_seconds = processed_seconds; @@ -81,24 +90,23 @@ rosbag::MessageInstance PlayableBag::GetNextMessage( bool PlayableBag::IsMessageAvailable() const { return !buffered_messages_.empty() && - (buffered_messages_.front().getTime() < - buffered_messages_.back().getTime() - buffer_delay_); + (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 (view_iterator_ == view_->end()) { + if (!bag_reader_->has_next()) { finished_ = true; return; } - rosbag::MessageInstance& msg = *view_iterator_; + auto msg = bag_reader_->read_next(); if (!filtering_early_message_handler_ || filtering_early_message_handler_(msg)) { - buffered_messages_.push_back(msg); + buffered_messages_.push_back(*msg); } - ++view_iterator_; ++message_counter_; } @@ -111,10 +119,15 @@ void PlayableBag::AdvanceUntilMessageAvailable() { } while (!finished_ && !IsMessageAvailable()); } -PlayableBagMultiplexer::PlayableBagMultiplexer() : pnh_("~") { - bag_progress_pub_ = pnh_.advertise( +PlayableBagMultiplexer::PlayableBagMultiplexer(rclcpp::Node::SharedPtr node) { + node_ = node; + bag_progress_pub_ = node_->create_publisher( "bagfile_progress", 10); - progress_pub_interval_ = pnh_.param("bagfile_progress_pub_interval", 10.0); + 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) { @@ -126,43 +139,51 @@ void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { next_message_queue_.emplace( BagMessageItem{playable_bags_.back().PeekMessageTime(), static_cast(playable_bags_.size() - 1)}); - bag_progress_time_map_[playable_bag.bag_id()] = ros::Time::now(); + bag_progress_time_map_[playable_bag.bag_id()] = node_->now(); } bool PlayableBagMultiplexer::IsMessageAvailable() const { return !next_message_queue_.empty(); } -std::tuple -PlayableBagMultiplexer::GetNextMessage() { +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::BagfileProgress progress; - rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress); + cartographer_ros_msgs::msg::BagfileProgress progress; + rosbag2_storage::SerializedBagMessage msg = current_bag.GetNextMessage(&progress); const bool publish_progress = current_bag.finished() || - ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >= - ros::Duration(progress_pub_interval_); - if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) { + 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()] = ros::Time::now(); + bag_progress_pub_->publish(progress); + bag_progress_time_map_[current_bag.bag_id()] = node_->now(); } - CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp); + 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}); } - return std::make_tuple(std::move(msg), current_bag.bag_id(), + + 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()); } -ros::Time PlayableBagMultiplexer::PeekMessageTime() const { +rclcpp::Time PlayableBagMultiplexer::PeekMessageTime() const { CHECK(IsMessageAvailable()); return next_message_queue_.top().message_timestamp; } diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/src/ros_log_sink.cpp similarity index 88% rename from cartographer_ros/cartographer_ros/ros_log_sink.cc rename to cartographer_ros/src/ros_log_sink.cpp index f5c7bbf52..d9b8ee2c1 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.cc +++ b/cartographer_ros/src/ros_log_sink.cpp @@ -22,7 +22,6 @@ #include #include "glog/log_severity.h" -#include "ros/console.h" namespace cartographer_ros { @@ -44,23 +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; // 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: - ROS_INFO_STREAM(message_string); + RCLCPP_INFO_STREAM(logger_, message_string); break; case ::google::GLOG_WARNING: - ROS_WARN_STREAM(message_string); + RCLCPP_WARN_STREAM(logger_, message_string); break; case ::google::GLOG_ERROR: - ROS_ERROR_STREAM(message_string); + RCLCPP_ERROR_STREAM(logger_, message_string); break; case ::google::GLOG_FATAL: - ROS_FATAL_STREAM(message_string); + 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 c5c0a1f87..a8a18d226 100644 --- a/cartographer_ros/cartographer_ros/ros_map.cc +++ b/cartographer_ros/src/ros_map.cpp @@ -16,15 +16,14 @@ #include "cartographer_ros/ros_map.h" -#include "absl/strings/str_cat.h" - namespace cartographer_ros { +// TODO: png ? void WritePgm(const ::cartographer::io::Image& image, const double resolution, ::cartographer::io::FileWriter* file_writer) { const std::string header = - absl::StrCat("P5\n# Cartographer map; ", resolution, " m/pixel\n", - image.width(), " ", image.height(), "\n255\n"); + "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) { @@ -39,10 +38,11 @@ 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 - const std::string output = absl::StrCat( - "image: ", pgm_filename, "\n", "resolution: ", resolution, "\n", - "origin: [", origin.x(), ", ", origin.y(), - ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"); + // 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()) + + ", 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 96% rename from cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc rename to cartographer_ros/src/ros_map_writing_points_processor.cpp index 5a4016e70..25c0e8312 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/src/ros_map_writing_points_processor.cpp @@ -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 57% rename from cartographer_ros/cartographer_ros/rosbag_validate_main.cc rename to cartographer_ros/src/rosbag_validate_main.cpp index c52c70049..a89ce437d 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/src/rosbag_validate_main.cpp @@ -25,19 +25,19 @@ #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 "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.h" -#include "tf2_msgs/TFMessage.h" +#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,12 +64,9 @@ 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 = absl::make_unique( @@ -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."; } @@ -251,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, 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."; + 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) { @@ -385,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 @@ -416,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 91% rename from cartographer_ros/cartographer_ros/sensor_bridge.cc rename to cartographer_ros/src/sensor_bridge.cpp index 3254d98a1..d1e4b18c9 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/src/sensor_bridge.cpp @@ -49,7 +49,7 @@ SensorBridge::SensorBridge( trajectory_builder_(trajectory_builder) {} std::unique_ptr SensorBridge::ToOdometryData( - const nav_msgs::Odometry::ConstPtr& msg) { + const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->child_frame_id)); @@ -62,7 +62,7 @@ std::unique_ptr SensorBridge::ToOdometryData( } void SensorBridge::HandleOdometryMessage( - const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { + const std::string& sensor_id, const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { std::unique_ptr odometry_data = ToOdometryData(msg); if (odometry_data != nullptr) { @@ -73,9 +73,9 @@ void SensorBridge::HandleOdometryMessage( } void SensorBridge::HandleNavSatFixMessage( - const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { + const std::string& sensor_id, const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); - if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) { + if (msg->status.status == sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX) { trajectory_builder_->AddSensorData( sensor_id, carto::sensor::FixedFramePoseData{time, absl::optional()}); @@ -99,7 +99,7 @@ void SensorBridge::HandleNavSatFixMessage( void SensorBridge::HandleLandmarkMessage( const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg) { auto landmark_data = ToLandmarkData(*msg); auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking( @@ -115,7 +115,7 @@ void SensorBridge::HandleLandmarkMessage( } std::unique_ptr SensorBridge::ToImuData( - const sensor_msgs::Imu::ConstPtr& msg) { + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) << "Your IMU data claims to not contain linear acceleration measurements " "by setting linear_acceleration_covariance[0] to -1. Cartographer " @@ -143,7 +143,7 @@ std::unique_ptr SensorBridge::ToImuData( } void SensorBridge::HandleImuMessage(const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg) { + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { std::unique_ptr imu_data = ToImuData(msg); if (imu_data != nullptr) { trajectory_builder_->AddSensorData( @@ -154,7 +154,7 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id, } void SensorBridge::HandleLaserScanMessage( - const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { + const std::string& sensor_id, const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); @@ -163,7 +163,7 @@ void SensorBridge::HandleLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage( const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); @@ -172,7 +172,7 @@ void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandlePointCloud2Message( const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); @@ -229,6 +229,10 @@ void SensorBridge::HandleRangefinder( 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) { @@ -236,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 111eb1654..40fa94f46 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/src/submap.cpp @@ -20,27 +20,37 @@ #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros_msgs/StatusCode.h" -#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, - ros::ServiceClient* client) { - ::cartographer_ros_msgs::SubmapQuery srv; - srv.request.trajectory_id = submap_id.trajectory_id; - srv.request.submap_index = submap_id.submap_index; - if (!client->call(srv) || - srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) { + 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; } - if (srv.response.textures.empty()) { + 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 = srv.response.submap_version; - for (const auto& texture : srv.response.textures) { + response->version = result->submap_version; + for (const auto& texture : result->textures) { const std::string compressed_cells(texture.cells.begin(), texture.cells.end()); response->textures.emplace_back(::cartographer::io::SubmapTexture{ diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/src/tf_bridge.cpp similarity index 86% rename from cartographer_ros/cartographer_ros/tf_bridge.cc rename to cartographer_ros/src/tf_bridge.cpp index 4b3c7838c..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 "absl/memory/memory.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 { @@ -31,19 +31,20 @@ TfBridge::TfBridge(const std::string& tracking_frame, std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( const ::cartographer::common::Time time, const std::string& frame_id) const { - ::ros::Duration timeout(lookup_transform_timeout_sec_); + tf2::Duration timeout(tf2::durationFromSec(lookup_transform_timeout_sec_)); std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; try { - const ::ros::Time latest_tf_time = + const rclcpp::Time latest_tf_time = buffer_ - ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.), + ->lookupTransform(tracking_frame_, frame_id, ::rclcpp::Time(0.), timeout) .header.stamp; - const ::ros::Time requested_time = ToRos(time); + const rclcpp::Time requested_time = ToRos(time); + 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 = ::ros::Duration(0.); + timeout = tf2::durationFromSec(0.0); } return absl::make_unique<::cartographer::transform::Rigid3d>( ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id, diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/src/time_conversion.cpp similarity index 75% rename from cartographer_ros/cartographer_ros/time_conversion.cc rename to cartographer_ros/src/time_conversion.cpp index 1075c40e8..c852aab41 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.cc +++ b/cartographer_ros/src/time_conversion.cpp @@ -17,31 +17,28 @@ #include "cartographer_ros/time_conversion.h" #include "cartographer/common/time.h" -#include "ros/ros.h" +#include namespace cartographer_ros { -::ros::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; - ::ros::Time ros_time; - ros_time.fromNSec(ns_since_unix_epoch); + 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 ::ros::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.nsec + 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/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index e4daaff63..a60f711ef 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -12,55 +12,61 @@ # See the License for the specific language governing permissions and # limitations under the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(cartographer_ros_msgs) -set(PACKAGE_DEPENDENCIES - geometry_msgs - std_msgs -) +# Default to C++14. +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES}) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # 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() -add_message_files( - DIRECTORY msg - FILES - BagfileProgress.msg - HistogramBucket.msg - LandmarkEntry.msg - LandmarkList.msg - MetricFamily.msg - MetricLabel.msg - Metric.msg - SensorTopics.msg - StatusCode.msg - StatusResponse.msg - SubmapEntry.msg - SubmapList.msg - SubmapTexture.msg - TrajectoryOptions.msg - TrajectoryStates.msg -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) -add_service_files( - DIRECTORY srv - FILES - FinishTrajectory.srv - GetTrajectoryStates.srv - ReadMetrics.srv - StartTrajectory.srv - SubmapQuery.srv - WriteState.srv +set(msg_files + "msg/BagfileProgress.msg" + "msg/HistogramBucket.msg" + "msg/LandmarkEntry.msg" + "msg/LandmarkList.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/TrajectoryStates.msg" ) -generate_messages( - DEPENDENCIES - ${PACKAGE_DEPENDENCIES} +set(srv_files + "srv/FinishTrajectory.srv" + "srv/GetTrajectoryStates.srv" + "srv/ReadMetrics.srv" + "srv/StartTrajectory.srv" + "srv/TrajectoryQuery.srv" + "srv/SubmapQuery.srv" + "srv/WriteState.srv" ) -catkin_package( - CATKIN_DEPENDS - ${PACKAGE_DEPENDENCIES} - message_runtime +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES geometry_msgs std_msgs + ADD_LINTER_TESTS ) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() 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/package.xml b/cartographer_ros_msgs/package.xml index 2e6825d04..1cbf10141 100644 --- a/cartographer_ros_msgs/package.xml +++ b/cartographer_ros_msgs/package.xml @@ -15,7 +15,7 @@ limitations under the License. --> - + cartographer_ros_msgs 1.0.0 @@ -26,18 +26,26 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors - catkin + ament_cmake + rosidl_default_generators geometry_msgs std_msgs - message_generation - - message_runtime + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/cartographer_ros_msgs/srv/ReadMetrics.srv b/cartographer_ros_msgs/srv/ReadMetrics.srv index 5070e3805..d6be66f07 100644 --- a/cartographer_ros_msgs/srv/ReadMetrics.srv +++ b/cartographer_ros_msgs/srv/ReadMetrics.srv @@ -15,4 +15,4 @@ --- cartographer_ros_msgs/StatusResponse status cartographer_ros_msgs/MetricFamily[] metric_families -time timestamp +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/msg/SensorTopics.msg b/cartographer_ros_msgs/srv/TrajectoryQuery.srv similarity index 72% rename from cartographer_ros_msgs/msg/SensorTopics.msg rename to cartographer_ros_msgs/srv/TrajectoryQuery.srv index 1db96864d..ca5da3b94 100644 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ b/cartographer_ros_msgs/srv/TrajectoryQuery.srv @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# 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. @@ -12,10 +12,7 @@ # 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 +int32 trajectory_id +--- +cartographer_ros_msgs/StatusResponse status +geometry_msgs/PoseStamped[] trajectory diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index c19c98675..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,100 +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) -if(WIN32) - set(Boost_USE_STATIC_LIBS FALSE) +# 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(Abseil REQUIRED) find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) - -catkin_package( - CATKIN_DEPENDS - message_runtime - ${PACKAGE_DEPENDENCIES} - INCLUDE_DIRS "." +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 ) -file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") -set(CMAKE_AUTOMOC ON) +include_directories( + include + ) -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::Core Qt5::Widgets) - include_directories(${Qt5Widgets_INCLUDE_DIRS}) -endif() -add_definitions(-DQT_NO_KEYWORDS) -add_library(${PROJECT_NAME} ${ALL_SRCS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) +add_library(${PROJECT_NAME} SHARED + src/drawable_submap.cpp + src/ogre_slice.cpp + src/submaps_display.cpp + ${rviz_plugins_headers_to_moc} + ) -# Add the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ +set(dependencies + rclcpp + cartographer + cartographer_ros + cartographer_ros_msgs + rviz2 + rviz_common + pcl_conversions + rcutils + ) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) -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}) - -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}) - -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) - -# On windows, rviz won't find the DLL in CATKIN_PACKAGE_BIN_DESTINATION, -# but it will in CATKIN_PACKAGE_LIB_DESTINATION? -if(WIN32) - set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -else() - set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -endif() +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 ${RUNTIME_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/cartographer_rviz/drawable_submap.h b/cartographer_rviz/include/cartographer_rviz/drawable_submap.h similarity index 76% rename from cartographer_rviz/cartographer_rviz/drawable_submap.h rename to cartographer_rviz/include/cartographer_rviz/drawable_submap.h index 21e19331d..fe9d5ba04 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/include/cartographer_rviz/drawable_submap.h @@ -19,7 +19,7 @@ #include #include - +#include #include "Eigen/Core" #include "Eigen/Geometry" #include "OgreSceneManager.h" @@ -29,27 +29,27 @@ #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, + ::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; @@ -58,12 +58,13 @@ class DrawableSubmap : public QObject { // 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(); @@ -103,14 +104,14 @@ class DrawableSubmap : public QObject { const ::cartographer::mapping::SubmapId id_; absl::Mutex mutex_; - ::rviz::DisplayContext* const display_context_; + ::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_rendering::Axes pose_axes_; bool pose_axes_visible_; - ::rviz::MovableText submap_id_text_; + ::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; @@ -118,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 61% rename from cartographer_rviz/cartographer_rviz/submaps_display.h rename to cartographer_rviz/include/cartographer_rviz/submaps_display.h index 7244b50e7..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 #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,11 +41,11 @@ 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::BoolProperty> pose_markers_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: @@ -60,13 +60,15 @@ 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; @@ -83,24 +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::map> trajectories_ GUARDED_BY(mutex_); absl::Mutex mutex_; - ::rviz::BoolProperty* slice_high_resolution_enabled_; - ::rviz::BoolProperty* slice_low_resolution_enabled_; - ::rviz::Property* trajectories_category_; - ::rviz::BoolProperty* visibility_all_enabled_; - ::rviz::BoolProperty* pose_markers_all_enabled_; - ::rviz::FloatProperty* fade_out_start_distance_in_meters_; + ::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 9b8bdcc65..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 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/cartographer_rviz/.clang-format b/cartographer_rviz/src/.clang-format similarity index 100% rename from cartographer_rviz/cartographer_rviz/.clang-format rename to cartographer_rviz/src/.clang-format diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/src/drawable_submap.cpp similarity index 87% rename from cartographer_rviz/cartographer_rviz/drawable_submap.cc rename to cartographer_rviz/src/drawable_submap.cpp index 2432ac243..c2b26db9e 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/src/drawable_submap.cpp @@ -26,9 +26,9 @@ #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,10 +44,11 @@ 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, + ::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) @@ -62,7 +63,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, .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(absl::make_unique( @@ -72,13 +73,13 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, // (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_ = absl::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); + 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(); @@ -96,8 +97,9 @@ DrawableSubmap::~DrawableSubmap() { } void DrawableSubmap::Update( - const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata) { + 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); @@ -114,7 +116,9 @@ void DrawableSubmap::Update( .arg(metadata_version_)); } -bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { +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 = @@ -130,9 +134,10 @@ 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_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000)); absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/src/ogre_slice.cpp similarity index 88% rename from cartographer_rviz/cartographer_rviz/ogre_slice.cc rename to cartographer_rviz/src/ogre_slice.cpp index 3062042d0..4be255419 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/src/ogre_slice.cpp @@ -24,7 +24,6 @@ #include "OgreMaterialManager.h" #include "OgreTechnique.h" #include "OgreTextureManager.h" -#include "absl/strings/str_cat.h" #include "cartographer/common/port.h" namespace cartographer_rviz { @@ -38,8 +37,7 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-", - slice_id); + return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id)); } } // namespace @@ -60,12 +58,11 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, scene_manager_(scene_manager), submap_node_(submap_node), slice_node_(submap_node_->createChildSceneNode()), - manual_object_(scene_manager_->createManualObject(absl::StrCat( - kManualObjectPrefix, GetSliceIdentifier(id, slice_id)))) { - material_ = Ogre::MaterialManager::getSingleton().getByName( - kSubmapSourceMaterialName); + manual_object_(scene_manager_->createManualObject( + kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { + material_ = Ogre::MaterialManager::getSingleton().getByName(kSubmapSourceMaterialName); material_ = material_->clone( - absl::StrCat(kSubmapMaterialPrefix, GetSliceIdentifier(id_, slice_id_))); + kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_), true, "General"); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -76,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_); @@ -119,14 +116,14 @@ 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 = - absl::StrCat(kSubmapTexturePrefix, GetSliceIdentifier(id_, slice_id_)); + kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, submap_texture.width, submap_texture.height, diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/src/submaps_display.cpp similarity index 74% rename from cartographer_rviz/cartographer_rviz/submaps_display.cc rename to cartographer_rviz/src/submaps_display.cpp index 76f5d2d49..edc8cd828 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/src/submaps_display.cpp @@ -19,17 +19,19 @@ #include "OgreResourceGroupManager.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,50 +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::BoolProperty( + 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_); } @@ -95,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() { @@ -108,18 +126,17 @@ void SubmapsDisplay::onInitialize() { void SubmapsDisplay::reset() { MFDClass::reset(); absl::MutexLock locker(&mutex_); - client_.shutdown(); + client_.reset(); trajectories_.clear(); CreateClient(); } -void SubmapsDisplay::processMessage( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { +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 (trajectories_.count(trajectory_id) == 0) { continue; @@ -136,7 +153,7 @@ void SubmapsDisplay::processMessage( using ::cartographer::mapping::SubmapId; std::set listed_submaps; std::set listed_trajectories; - for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + 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); listed_trajectories.insert(submap_entry.trajectory_id); @@ -144,7 +161,7 @@ void SubmapsDisplay::processMessage( trajectories_.insert(std::make_pair( id.trajectory_id, absl::make_unique( - absl::make_unique<::rviz::BoolProperty>( + absl::make_unique<::rviz_common::properties::BoolProperty>( QString("Trajectory %1").arg(id.trajectory_id), visibility_all_enabled_->getBool(), QString( @@ -201,7 +218,7 @@ void SubmapsDisplay::processMessage( } } -void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { +void SubmapsDisplay::update(const float , const float) { absl::MutexLock locker(&mutex_); // Schedule fetching of new submap textures. for (const auto& trajectory_by_id : trajectories_) { @@ -215,7 +232,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { 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; } } @@ -223,12 +240,13 @@ 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); + 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( @@ -237,12 +255,12 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } } } 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); @@ -292,14 +310,14 @@ void Trajectory::PoseMarkersEnabledToggled() { } } -Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property, +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::BoolProperty>( + 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()); @@ -309,4 +327,4 @@ Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property, } // 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 index a5da0fba0..42b056a57 100644 --- a/docs/source/algo_walkthrough.rst +++ b/docs/source/algo_walkthrough.rst @@ -31,8 +31,8 @@ IEEE, 2016. pp. 1271–1278. Overview -------- -.. image:: https://raw.githubusercontent.com/googlecartographer/cartographer/master/docs/source/high_level_system_overview.png - :target: https://github.com/googlecartographer/cartographer/blob/master/docs/source/high_level_system_overview.png +.. 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). @@ -40,8 +40,8 @@ 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/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_2d.lua -.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua: https://github.com/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_3d.lua +.. _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**. @@ -50,7 +50,7 @@ It also incorporates other sensor data to get a higher level view and identify t 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/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/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. @@ -276,9 +276,9 @@ The resulting net is called the "*pose graph*". 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 inter submaps constraints) are built automatically between nodes that are closely following each other on a trajectory. +- 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 intra 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). +- 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 diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index cf5a4da76..c0315fe42 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -36,7 +36,7 @@ The assets writer takes as input 4. and a pipeline configuration, which is defined in a ``.lua`` file. The assets writer runs through the ``.bag`` data in batches with the trajectory found in the ``.pbstream``. -The pipeline can be can be used to color, filter and export SLAM point cloud data into a variety of formats. +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 @@ -62,11 +62,11 @@ When running as an online node, Cartographer doesn't know when your bag (or sens # Ask Cartographer to serialize its current state. # (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'}" + rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: "true"}" Once you've retrieved your ``.pbstream`` file, you can run the assets writer with the `sample pipeline`_ for the 3D backpack: -.. _sample pipeline: 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 @@ -80,20 +80,20 @@ For the last example, if you specify ``points.ply`` in the pipeline configuratio Configuration ------------- -The assets writer is modeled as a pipeline of `PointsProcessor`_s. -`PointsBatch`_\ s flow through each processor and they all have 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`_ 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 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/f1ac8967297965b8eb6f2f4b08a538e052b5a75b/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. @@ -105,7 +105,7 @@ The available ``PointsProcessor``\ s are all defined in the `cartographer/io`_ s * **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 cofnigure the range data ray tracing through the probability grid. +* **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. @@ -115,18 +115,19 @@ First-person visualization of point clouds 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/googlecartographer/point_cloud_viewer +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer .. _meshlab: http://www.meshlab.net/ 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`_. -.. _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 +.. _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 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. -.. _point_cloud_viewer: https://github.com/googlecartographer/point_cloud_viewer +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer .. image:: point_cloud_viewer_demo_3d.jpg diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 3d4a3be43..ace433fd5 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -23,10 +23,9 @@ The Cartographer ROS requirements are the same as `the ones from Cartographer`_. The following `ROS distributions`_ are currently supported: -* Indigo * Kinetic -* Lunar * Melodic +* Noetic .. _the ones from Cartographer: https://google-cartographer.readthedocs.io/en/latest/#system-requirements .. _ROS distributions: http://wiki.ros.org/Distributions @@ -38,33 +37,61 @@ In order to build Cartographer ROS, we recommend using `wstool `_. 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 python-wstool python-rosdep ninja-build + sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow -Create a new cartographer_ros workspace in 'catkin_ws'. +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/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall + wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall wstool update -t src -Install cartographer_ros' dependencies (proto3 and deb packages). +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 - src/cartographer/scripts/install_proto3.sh 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/configuration.rst b/docs/source/configuration.rst index 1975e8b73..67a59e0a4 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -103,6 +103,12 @@ 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. @@ -124,12 +130,13 @@ landmarks_sampling_ratio .. _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/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/LandmarkList.msg -.. _cartographer_ros_msgs/LandmarkEntry: https://github.com/googlecartographer/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkEntry.msg +.. _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/demos.rst b/docs/source/demos.rst index 4e2e19e16..dac214f36 100644 --- a/docs/source/demos.rst +++ b/docs/source/demos.rst @@ -81,7 +81,7 @@ Generate the map (wait until cartographer_offline_node finishes) and then run pu bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag Static landmarks -======== +================ .. raw:: html 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/getting_involved.rst b/docs/source/getting_involved.rst index 3e4509777..6ada0854e 100644 --- a/docs/source/getting_involved.rst +++ b/docs/source/getting_involved.rst @@ -19,28 +19,17 @@ Getting involved Cartographer is developed in the open and allows anyone to contribute to the project. There are multiple ways to get involved! -Twice a month, the project hosts "Open House Hangouts" sessions that are essentially meetings open to everyone to join in. -The call typically recaps the recent and ongoing development around Cartographer and Cartographer ROS. -The developers are then open to questions from the community, this is a great time to ask about contribution ideas. -If you don't feel like talking or being seen, you are free to join anyway and skulk! -The slides are also made available after each session but there is no video recording. +If you have question or think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. -If you want to stay tuned with announcements (such as new major releases or new open house sessions), you can join `the Cartographer mailing list`_ although you can not interact with this mailing list anymore. - -.. _the Cartographer mailing list: https://groups.google.com/forum/#!forum/google-cartographer - -If you think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_ but don't forget to provide a way to reproduce your bug! -Typically, join a ``.bag`` and a link to a fork of the ``cartographer_ros`` repository containing your configuration and launch files. - -.. _GitHub issue: https://github.com/googlecartographer/cartographer/issues +.. _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/googlecartographer/rfcs +.. _the RFCs repository: https://github.com/cartographer-project/rfcs If you want to contribute code or documentation, this is done through `GitHub pull requests`_. -However, make sure you have signed (online) the `Contributor License Agreement`_ first! +Pull requests need to follow the `contribution guidelines`_. -.. _GitHub pull requests: https://github.com/googlecartographer/cartographer/pulls -.. _Contributor License Agreement: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md +.. _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 index 47c803ffd..3e94757ac 100644 --- a/docs/source/going_further.rst +++ b/docs/source/going_further.rst @@ -42,7 +42,7 @@ For landmarks publishing on a ``cartographer_ros_msgs/LandmarkList`` (`message d use_landmarks = true -.. _message defined in cartographer_ros: https://github.com/googlecartographer/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg +.. _message defined in cartographer_ros: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg Localization only ================= diff --git a/docs/source/index.rst b/docs/source/index.rst index 0cbb5e406..03b85c02f 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -38,5 +38,5 @@ configurations. This project provides Cartographer's ROS integration. data faq -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 263d1090c..7227d016a 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -23,7 +23,7 @@ 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 ------------------ @@ -82,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 -------- @@ -94,9 +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. - The ``start_trajectory`` executable provides a convenient wrapper to use this service. + 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. @@ -129,21 +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/GetTrajectoryStates: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv -.. _cartographer_ros_msgs/ReadMetrics: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.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 @@ -161,7 +169,7 @@ 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 @@ -173,7 +181,7 @@ In addition to the topics that are published by the online node, this node also 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/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg +.. _cartographer_ros_msgs/BagfileProgress: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg Parameters ---------- @@ -191,7 +199,7 @@ Generating the map is expensive and slow, so map updates are in the order of sec 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 ----------------- @@ -213,7 +221,7 @@ 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/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +.. _pbstream_map_publisher: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc Subscribed Topics ----------------- diff --git a/docs/source/tuning.rst b/docs/source/tuning.rst index fd4f7702e..53ad0d315 100644 --- a/docs/source/tuning.rst +++ b/docs/source/tuning.rst @@ -46,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. @@ -103,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 ------------- @@ -203,7 +203,7 @@ Developers are keen to help, but they can only be helpful if you follow `an issu 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/googlecartographer/cartographer_ros/issues -.. _an issue template: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question -.. _the closed issues of cartographer_ros: https://github.com/googlecartographer/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed -.. _of cartographer: https://github.com/googlecartographer/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed +.. _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 index 9148e9895..8d33e0fd0 100644 --- a/docs/source/your_bag.rst +++ b/docs/source/your_bag.rst @@ -113,7 +113,7 @@ Start by copying one of the provided example: - ``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 ``.pstream`` recording of a previous Cartographer execution. +- ``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. 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 9cc45d766..0af46489a 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -17,29 +17,20 @@ set -o errexit set -o verbose -# Install CMake 3.2 for Ubuntu Trusty and Debian Jessie. +# Install CMake, Ninja, stow. sudo apt-get update -sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "trusty" ]] -then - sudo apt-get install cmake3 -y -elif [[ "$(lsb_release -sc)" = "jessie" ]] +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 sh -c "echo 'deb http://ftp.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" - sudo apt-get update - sudo apt-get -t jessie-backports install cmake -y -else - sudo apt-get install cmake -y + 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 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 diff --git a/scripts/update_catkin_workspace.sh b/scripts/update_catkin_workspace.sh deleted file mode 100755 index 15807cd6d..000000000 --- a/scripts/update_catkin_workspace.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/sh - -# 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. - -set -o errexit -set -o verbose - -. /opt/ros/${ROS_DISTRO}/setup.sh - -cd catkin_ws/src - -# Call 'status' as a workaround for https://github.com/vcstools/wstool/issues/77 -wstool status -wstool update