m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(slam): add cartographer_ros

Browse Source
This commit is contained in:
X-lanni
2025-07-04 14:39:48 +08:00
parent df9cee5779
commit 5cf886315d
825 changed files with 92220 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cartographer_ros/.dockerignore
+4
View File
@@ -0,0 +1,4 @@
**/Dockerfile*
**/.dockerignore
**/.git
**/.travis.yml
cartographer_ros/.github/ISSUE_TEMPLATE/bug-report.md
Vendored
+19
View File
@@ -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 <package_name>`):
- 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.
cartographer_ros/.github/ISSUE_TEMPLATE/general-issue.md
Vendored
+5
View File
@@ -0,0 +1,5 @@
---
name: General issue
about: Use this issue for general questions.
---
cartographer_ros/.github/ISSUE_TEMPLATE/tuning-advice.md
Vendored
+30
View File
@@ -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 <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.
cartographer_ros/.github/PULL_REQUEST_TEMPLATE.md
Vendored
+2
View File
@@ -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).
cartographer_ros/.travis.yml
+38
View File
@@ -0,0 +1,38 @@
# 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.
sudo: required
services: docker
# Cache intermediate Docker layers. For a description of how this works, see:
# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html
cache:
directories:
- /home/travis/docker/
env:
- ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-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).
# Either add your token to the 'env' section above or add it as an unencrypted variable in the Travis settings.
- scripts/check_access_token.sh $GITHUB_TOKEN
- scripts/load_docker_cache.sh
install: true
script:
- docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros:${ROS_RELEASE} -f Dockerfile.${ROS_RELEASE} --build-arg github_token=${GITHUB_TOKEN}
- scripts/save_docker_cache.sh
cartographer_ros/AUTHORS
+7
View File
@@ -0,0 +1,7 @@
# This is the list of Cartographer authors for copyright purposes.
#
# This does not necessarily list everyone who has contributed code, since in
# some cases, their employer may be the copyright holder. To see the full list
# of contributors, see the revision history in source control.
Google Inc.
and other contributors
cartographer_ros/CONTRIBUTING.md
+51
View File
@@ -0,0 +1,51 @@
Want to contribute? Great! First, read this page.
### Before you contribute
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. 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.
cartographer_ros/Dockerfile.kinetic
+80
View File
@@ -0,0 +1,80 @@
# 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 osrf/ros:kinetic-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
# 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/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/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
# 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
# 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/
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
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 /
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
cartographer_ros/Dockerfile.melodic
+79
View File
@@ -0,0 +1,79 @@
# 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 osrf/ros:melodic-desktop
ARG CARTOGRAPHER_VERSION=master
# We require a GitHub access token to be passed.
ARG github_token
# Bionic's base image doesn't ship with sudo.
RUN apt-get update && apt-get install -y sudo
# 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/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/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
# 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
# determined by Catkin.
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
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 /
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
cartographer_ros/Dockerfile.noetic
+83
View File
@@ -0,0 +1,83 @@
# 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.
# 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 osrf/ros:noetic-desktop
ARG CARTOGRAPHER_VERSION=master
# We require a GitHub access token to be passed.
ARG github_token
# Prevent any interaction required by apt-get.
# https://stackoverflow.com/questions/22466255
ARG DEBIAN_FRONTEND=noninteractive
# 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/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/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
# 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
# determined by Catkin.
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
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 /
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
cartographer_ros/LICENSE
+202
View File
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
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.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
cartographer_ros/README.rst
+60
View File
@@ -0,0 +1,60 @@
.. 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.
============================
Cartographer ROS Integration
============================
|build| |docs| |license|
Purpose
=======
`Cartographer`_ is a system that provides real-time simultaneous localization
and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor
configurations. This project provides Cartographer's ROS integration.
.. _Cartographer: https://github.com/cartographer-project/cartographer
.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping
Getting started
===============
* Learn to use Cartographer with ROS at `our Read the Docs site`_.
* 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/cartographer-project/cartographer_ros/issues/new?labels=question
Contributing
============
You can find information about contributing to Cartographer's ROS integration
at `our Contribution page`_.
.. _our Contribution page: https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md
.. |build| image:: https://travis-ci.org/cartographer-project/cartographer_ros.svg?branch=master
:alt: Build Status
:scale: 100%
: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%
:target: https://google-cartographer-ros.readthedocs.io/en/latest/?badge=latest
.. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg
:alt: Apache 2 license.
:scale: 100%
:target: https://github.com/cartographer-project/cartographer_ros/blob/master/LICENSE
cartographer_ros/azure-pipelines.yml
+70
View File
@@ -0,0 +1,70 @@
# Copyright 2018 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
jobs:
- job: Build
pool:
vmImage: 'vs2017-win2016'
timeoutInMinutes: 360
steps:
- script: |
choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1
rem Azure VM runs out of space on C:, so use D: for ros and rosdeps
mkdir D:\opt && mklink /J C:\opt D:\opt
choco upgrade %ROS_METAPACKAGE% -y
choco upgrade ros-melodic-perception -y
robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL
git clone https://github.com/cartographer-project/cartographer src\cartographer
call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y
env:
ROS_METAPACKAGE: 'ros-melodic-desktop'
displayName: Install prerequisites
- script: |
call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
call "C:\opt\ros\melodic\x64\setup.bat"
call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release
displayName: Build
- script: |
call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
call "C:\opt\ros\melodic\x64\setup.bat"
call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
cd build_isolated\cartographer\install && ctest --no-compress-output -T Test
displayName: Run cartographer tests
- script: |
call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
call "C:\opt\ros\melodic\x64\setup.bat"
call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test
displayName: Build and run cartographer_ros tests
condition: always()
- script: |
call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
call "C:\opt\ros\melodic\x64\setup.bat"
call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros
displayName: Convert tests to jUnit
condition: always()
- task: PublishTestResults@2
displayName: Publish test results
inputs:
testRunner: 'jUnit'
testResultsFiles: '**\jUnit.xml'
searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing'
condition: always()
cartographer_ros/cartographer_ros.rosinstall
+2
View File
@@ -0,0 +1,2 @@
- 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'}
cartographer_ros/cartographer_ros/CHANGELOG.rst
+39
View File
@@ -0,0 +1,39 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package cartographer_ros
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2.0.9003 (2024-04-17)
---------------------
* Fix build on Rolling. (`#74 <https://github.com/ros2/cartographer_ros/issues/74>`_)
* Contributors: Chris Lalancette
2.0.9002 (2024-03-15)
---------------------
* Fix for newer Google logging.
* Fix warnings when building against recent ROS 2 releases.
* Contributors: Chris Lalancette
2.0.9001 (2023-04-13)
---------------------
* fix cmake to prevent multiple definitions (`#63 <https://github.com/ros2/cartographer_ros/issues/63>`_)
* Contributors: Wolf Vollprecht
2.0.9000 (2022-03-31)
---------------------
* Update to latest upstream, as well as new port to ROS 2.
1.0.0 (2018-06-01)
------------------
* https://github.com/googlecartographer/cartographer_ros/compare/0.3.0...1.0.0
0.3.0 (2017-11-23)
------------------
* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0
0.2.0 (2017-06-19)
------------------
* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0
0.1.0 (2017-05-18)
------------------
* First unstable development release
cartographer_ros/cartographer_ros/CMakeLists.txt
+333
View File
@@ -0,0 +1,333 @@
# 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.
cmake_minimum_required(VERSION 3.5)
project(cartographer_ros)
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(absl REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(cartographer REQUIRED)
find_package(cartographer_ros_msgs REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(gflags REQUIRED)
find_package(glog REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(PCL REQUIRED COMPONENTS common)
find_package(pcl_conversions REQUIRED)
find_package(rclcpp REQUIRED)
find_package(ros_environment REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(rosbag2_storage REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(urdf REQUIRED)
find_package(visualization_msgs REQUIRED)
# glog is not linked, however we look for it to detect the glog version
# and use a different code path if glog >= 0.7.0 is detected
find_package(glog CONFIG QUIET)
if(DEFINED glog_VERSION)
if(NOT glog_VERSION VERSION_LESS 0.7.0)
add_definitions(-DROS_CARTOGRAPHER_GLOG_GE_070)
endif()
endif()
include(FindPkgConfig)
pkg_search_module(CAIRO REQUIRED cairo>=1.12.16)
# Library
add_library(${PROJECT_NAME} STATIC
src/assets_writer.cpp
src/map_builder_bridge.cpp
src/msg_conversion.cpp
src/node_constants.cpp
src/node.cpp
src/node_options.cpp
src/offline_node.cpp
src/playable_bag.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
)
target_include_directories(${PROJECT_NAME} PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
${CAIRO_INCLUDE_DIRS}
${pcl_conversions_INCLUDE_DIRS}
)
if ("$ENV{ROS_DISTRO}" STRLESS "kilted")
# In Kilted, pcl_conversions was converted over to an INTERFACE target,
# but prior to it it is only available as the INCLUDE_DIRS. So we have this
# compatibility hack to deal with that difference.
target_include_directories(${PROJECT_NAME} PRIVATE ${pcl_conversions_INCLUDE_DIRS})
# In Kilted, urdf/model.h was deprecated in favor of urdf/model.hpp.
# Deal with the difference here so we have no warnings.
target_compile_definitions(${PROJECT_NAME} PRIVATE "-DUSE_URDF_H_FILES")
else()
target_link_libraries(${PROJECT_NAME} PRIVATE pcl_conversions::pcl_conversions)
endif()
target_link_libraries(${PROJECT_NAME} PUBLIC
absl::synchronization
${builtin_interfaces_TARGETS}
cartographer
${cartographer_ros_msgs_TARGETS}
${geometry_msgs_TARGETS}
${nav_msgs_TARGETS}
rclcpp::rclcpp
rosbag2_cpp::rosbag2_cpp
rosbag2_storage::rosbag2_storage
${sensor_msgs_TARGETS}
tf2_ros::tf2_ros
${visualization_msgs_TARGETS}
)
target_link_libraries(${PROJECT_NAME} PRIVATE
${CAIRO_LIBRARIES}
pcl_common
${std_msgs_TARGETS}
tf2::tf2
tf2_eigen::tf2_eigen
${tf2_msgs_TARGETS}
urdf::urdf
)
# Executables
add_executable(cartographer_node src/node_main.cpp)
target_include_directories(cartographer_node PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
target_link_libraries(cartographer_node PRIVATE
${PROJECT_NAME}
absl::memory
cartographer
gflags
rclcpp::rclcpp
tf2_ros::tf2_ros
)
add_executable(cartographer_occupancy_grid_node src/occupancy_grid_node_main.cpp)
target_include_directories(cartographer_occupancy_grid_node PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
${CAIRO_INCLUDE_DIRS}
)
target_link_libraries(cartographer_occupancy_grid_node PRIVATE
${PROJECT_NAME}
absl::synchronization
cartographer
${cartographer_ros_msgs_TARGETS}
Eigen3::Eigen
gflags
${nav_msgs_TARGETS}
rclcpp::rclcpp
)
add_executable(cartographer_offline_node src/offline_node_main.cpp)
target_include_directories(cartographer_offline_node PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
target_link_libraries(cartographer_offline_node PRIVATE
${PROJECT_NAME}
cartographer
gflags
rclcpp::rclcpp
)
add_executable(cartographer_assets_writer src/assets_writer_main.cpp)
target_include_directories(cartographer_assets_writer PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
target_link_libraries(cartographer_assets_writer PRIVATE
${PROJECT_NAME}
gflags
glog::glog
rclcpp::rclcpp
)
add_executable(cartographer_pbstream_map_publisher src/pbstream_map_publisher_main.cpp)
target_include_directories(cartographer_pbstream_map_publisher PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
target_link_libraries(cartographer_pbstream_map_publisher PRIVATE
${PROJECT_NAME}
cartographer
gflags
glog::glog
${nav_msgs_TARGETS}
rclcpp::rclcpp
)
add_executable(cartographer_pbstream_to_ros_map src/pbstream_to_ros_map_main.cpp)
target_include_directories(cartographer_pbstream_to_ros_map PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
target_link_libraries(cartographer_pbstream_to_ros_map PRIVATE
${PROJECT_NAME}
cartographer
gflags
glog::glog
rclcpp::rclcpp
)
add_executable(cartographer_rosbag_validate src/rosbag_validate_main.cpp)
target_include_directories(cartographer_rosbag_validate PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
if ("$ENV{ROS_DISTRO}" STRLESS "kilted")
target_compile_definitions(cartographer_rosbag_validate PRIVATE "-DUSE_URDF_H_FILES")
endif()
target_link_libraries(cartographer_rosbag_validate
${PROJECT_NAME}
absl::memory
cartographer
gflags
glog::glog
${nav_msgs_TARGETS}
rclcpp::rclcpp
rosbag2_cpp::rosbag2_cpp
${sensor_msgs_TARGETS}
tf2_eigen::tf2_eigen
${tf2_msgs_TARGETS}
tf2_ros::tf2_ros
urdf::urdf
tf2::tf2
)
if($ENV{ROS_DISTRO} MATCHES "humble" OR $ENV{ROS_DISTRO} MATCHES "iron")
target_compile_definitions(${PROJECT_NAME} PRIVATE PRE_JAZZY_SERIALIZED_BAG_MSG_FIELD_NAME)
target_compile_definitions(cartographer_rosbag_validate PRIVATE PRE_JAZZY_SERIALIZED_BAG_MSG_FIELD_NAME)
endif()
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(TARGETS
cartographer_node
cartographer_occupancy_grid_node
cartographer_offline_node
cartographer_assets_writer
cartographer_pbstream_map_publisher
cartographer_pbstream_to_ros_map
cartographer_rosbag_validate
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(DIRECTORY configuration_files urdf launch
DESTINATION share/${PROJECT_NAME}/
)
ament_export_targets(${PROJECT_NAME})
ament_export_include_directories(include/${PROJECT_NAME})
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(
absl
builtin_interfaces
cartographer
cartographer_ros_msgs
geometry_msgs
nav_msgs
rclcpp
rosbag2_cpp
rosbag2_storage
sensor_msgs
tf2_ros
visualization_msgs
)
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()
cartographer_ros/cartographer_ros/configuration_files/agvpro_backpack_2d.lua
+50
View File
@@ -0,0 +1,50 @@
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = true,
use_pose_extrapolator = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
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_2D.num_accumulated_range_data = 10
TRAJECTORY_BUILDER_2D.use_imu_data = false
return options
cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua
+131
View File
@@ -0,0 +1,131 @@
-- 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.
-- WARNING: we create a lot of X-Rays of a potentially large space in this
-- pipeline. For example, running over the
-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can
-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size
-- of a pixel in a X-Ray.
VOXEL_SIZE = 5e-2
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
-- Gray X-Rays. These only use geometry to color pixels.
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all",
transform = XZ_TRANSFORM,
},
-- We now use the intensities to color our points. We apply a linear
-- transform to clamp our intensity values into [0, 255] and then use this
-- value for RGB of our points. Every stage in the pipeline after this now
-- receives colored points.
--
-- We write xrays again. These now use geometry and the intensities to
-- color pixels - they look quite similar, just a little lighter.
{
action = "intensity_to_color",
min_intensity = 0.,
max_intensity = 4095.,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all_intensity",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all_intensity",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all_intensity",
transform = XZ_TRANSFORM,
},
-- 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/cartographer-project/point_cloud_viewer.
{
action = "write_ply",
filename = "points.ply",
},
-- Now we recolor our points by frame and write another batch of X-Rays. It
-- is visible in them what was seen by the horizontal and the vertical
-- laser.
{
action = "color_points",
frame_id = "horizontal_laser_link",
color = { 255., 0., 0. },
},
{
action = "color_points",
frame_id = "vertical_laser_link",
color = { 0., 255., 0. },
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all_color",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all_color",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all_color",
transform = XZ_TRANSFORM,
},
}
}
return options
cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_2d_ci.lua
+56
View File
@@ -0,0 +1,56 @@
-- 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.
-- WARNING: we create a lot of X-Rays of a potentially large space in this
-- pipeline. For example, running over the
-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can
-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size
-- of a pixel in a X-Ray.
VOXEL_SIZE = 5e-2
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all",
transform = XZ_TRANSFORM,
},
}
}
return options
cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua
+86
View File
@@ -0,0 +1,86 @@
-- 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.
VOXEL_SIZE = 5e-2
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
-- Gray X-Rays. These only use geometry to color pixels.
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all",
transform = XZ_TRANSFORM,
},
-- Now we recolor our points by frame and write another batch of X-Rays. It
-- is visible in them what was seen by the horizontal and the vertical
-- laser.
{
action = "color_points",
frame_id = "horizontal_vlp16_link",
color = { 255., 0., 0. },
},
{
action = "color_points",
frame_id = "vertical_vlp16_link",
color = { 0., 255., 0. },
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all_color",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all_color",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all_color",
transform = XZ_TRANSFORM,
},
}
}
return options
cartographer_ros/cartographer_ros/configuration_files/assets_writer_ros_map.lua
+36
View File
@@ -0,0 +1,36 @@
-- 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.
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "write_ros_map",
range_data_inserter = {
insert_free_space = true,
hit_probability = 0.55,
miss_probability = 0.49,
},
filestem = "map",
resolution = 0.05,
}
}
}
return options
cartographer_ros/cartographer_ros/configuration_files/backpack_2d.lua
+49
View File
@@ -0,0 +1,49 @@
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 1,
num_subdivisions_per_laser_scan = 10,
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_2D.num_accumulated_range_data = 10
return options
cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization.lua
+22
View File
@@ -0,0 +1,22 @@
-- 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 "backpack_2d.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20
return options
cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua
+27
View File
@@ -0,0 +1,27 @@
-- 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 "backpack_2d_localization.lua"
-- output map to base_link for evaluation
options.provide_odom_frame = false
POSE_GRAPH.optimization_problem.log_solver_summary = true
-- fast localization
MAP_BUILDER.num_background_threads = 12
POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio
POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio
POSE_GRAPH.max_num_final_iterations = 1
return options
cartographer_ros/cartographer_ros/configuration_files/backpack_2d_server.lua
+19
View File
@@ -0,0 +1,19 @@
-- 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_server.lua"
MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
return MAP_BUILDER_SERVER
cartographer_ros/cartographer_ros/configuration_files/backpack_3d.lua
+57
View File
@@ -0,0 +1,57 @@
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 2,
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.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
return options
cartographer_ros/cartographer_ros/configuration_files/backpack_3d_localization.lua
+22
View File
@@ -0,0 +1,22 @@
-- 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 "backpack_3d.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 100
return options
cartographer_ros/cartographer_ros/configuration_files/demo_2d.rviz
+459
View File
@@ -0,0 +1,459 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /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_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: 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: ""
Visualization Manager:
Class: ""
Displays:
- 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
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
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
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
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: 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.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:
{}
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
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: nav2_rviz_plugins/GoalTool
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 204.58663940429688
Enable Stereo Rendering:
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.009999999776482582
Pitch: 1.5697963237762451
Target Frame: map
Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 1.6953976154327393
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1403
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000028100000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000006640000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Teleop:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2560
Wyca:
collapsed: false
X: 3840
Y: 0
cartographer_ros/cartographer_ros/configuration_files/demo_3d.rviz
+394
View File
@@ -0,0 +1,394 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /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_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: 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: matched_points
Visualization Manager:
Class: ""
Displays:
- 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
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
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base_link:
Value: false
horizontal_vlp16_link:
Value: true
imu_link:
Value: true
map:
Value: true
odom:
Value: true
vertical_vlp16_link:
Value: true
Marker Scale: 0.30000001192092896
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
map:
odom:
base_link:
horizontal_vlp16_link:
{}
imu_link:
{}
vertical_vlp16_link:
{}
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
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: matched_points
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
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: 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: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 57
Min Color: 0; 0; 0
Min Intensity: 1
Name: horizontal_laser
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
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
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 90
Min Color: 0; 0; 0
Min Intensity: 1
Name: vertical_laser
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
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: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: nav2_rviz_plugins/GoalTool
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 59.301063537597656
Enable Stereo Rendering:
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.009999999776482582
Pitch: 1.1647964715957642
Target Frame: map
Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 0.3154185116291046
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1403
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000007390000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Teleop:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2560
Wyca:
collapsed: false
X: 3840
Y: 0
cartographer_ros/cartographer_ros/configuration_files/mir-100-mapping.lua
+77
View File
@@ -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
cartographer_ros/cartographer_ros/configuration_files/pr2.lua
+55
View File
@@ -0,0 +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.
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
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_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
POSE_GRAPH.optimization_problem.huber_scale = 1e2
return options
cartographer_ros/cartographer_ros/configuration_files/revo_lds.lua
+62
View File
@@ -0,0 +1,62 @@
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "horizontal_laser_link",
published_frame = "horizontal_laser_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
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_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
cartographer_ros/cartographer_ros/configuration_files/taurob_tracker.lua
+60
View File
@@ -0,0 +1,60 @@
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
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.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180
TRAJECTORY_BUILDER_3D.min_range = 0.5
TRAJECTORY_BUILDER_3D.max_range = 20.
TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40.
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 40
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.log_matches = true
return options
cartographer_ros/cartographer_ros/configuration_files/transform.lua
+28
View File
@@ -0,0 +1,28 @@
-- Copyright 2017 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.
XY_TRANSFORM = {
translation = { 0., 0., 0. },
rotation = { 0., -math.pi / 2., 0., },
}
XZ_TRANSFORM = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., -math.pi / 2, },
}
YZ_TRANSFORM = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., math.pi, },
}
cartographer_ros/cartographer_ros/configuration_files/visualize_pbstream.lua
+21
View File
@@ -0,0 +1,21 @@
-- 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 "backpack_2d.lua"
POSE_GRAPH.constraint_builder.sampling_ratio = 0
POSE_GRAPH.global_sampling_ratio = 0
POSE_GRAPH.optimize_every_n_nodes = 0
return options
cartographer_ros/cartographer_ros/include/cartographer_ros/assets_writer.h
+64
View File
@@ -0,0 +1,64 @@
/*
* 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 <string>
#include <vector>
#include "cartographer/common/configuration_file_resolver.h"
#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 <rclcpp/rclcpp.hpp>
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
namespace cartographer_ros {
class AssetsWriter {
public:
AssetsWriter(const std::string& pose_graph_filename,
const std::vector<std::string>& bag_filenames,
const std::string& output_file_prefix);
// Registers a new PointsProcessor type uniquly identified by 'name' which
// will be created using 'factory'.
void RegisterPointsProcessor(
const std::string& name,
cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction
factory);
// Configures a points processing pipeline and pushes the points from the
// bag through the pipeline.
void Run(const std::string& configuration_directory,
const std::string& configuration_basename,
const std::string& urdf_filename, bool use_bag_transforms);
// Creates a FileWriterFactory which creates a FileWriter for storing assets.
static ::cartographer::io::FileWriterFactory CreateFileWriterFactory(
const std::string& file_path);
private:
std::vector<std::string> bag_filenames_;
std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_;
::cartographer::mapping::proto::PoseGraph pose_graph_;
std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder>
point_pipeline_builder_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
cartographer_ros/cartographer_ros/include/cartographer_ros/map_builder_bridge.h
+130
View File
@@ -0,0 +1,130 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
#include <memory>
#include <set>
#include <string>
#include <unordered_map>
#include "absl/synchronization/mutex.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/trajectory_options.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::msg::Marker::DELETE.
#ifdef DELETE
#undef DELETE
#endif
#include "visualization_msgs/msg/marker_array.hpp"
namespace cartographer_ros {
class MapBuilderBridge {
public:
struct LocalTrajectoryData {
// Contains the trajectory data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'.
struct LocalSlamData {
::cartographer::common::Time time;
::cartographer::transform::Rigid3d local_pose;
::cartographer::sensor::RangeData range_data_in_local;
};
std::shared_ptr<const LocalSlamData> local_slam_data;
cartographer::transform::Rigid3d local_to_map;
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
TrajectoryOptions trajectory_options;
};
MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* tf_buffer);
MapBuilderBridge(const MapBuilderBridge&) = delete;
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
void LoadState(const std::string& state_filename, bool load_frozen_state);
int AddTrajectory(
const std::set<
::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id);
void RunFinalOptimization();
bool SerializeState(const std::string& filename,
const bool include_unfinished_submaps);
void HandleSubmapQuery(
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<int /* trajectory_id */,
::cartographer::mapping::PoseGraphInterface::TrajectoryState>
GetTrajectoryStates();
cartographer_ros_msgs::msg::SubmapList GetSubmapList(rclcpp::Time node_time);
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
LOCKS_EXCLUDED(mutex_);
visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(rclcpp::Time node_time);
visualization_msgs::msg::MarkerArray GetLandmarkPosesList(rclcpp::Time node_time);
visualization_msgs::msg::MarkerArray GetConstraintList(rclcpp::Time node_time);
SensorBridge* sensor_bridge(int trajectory_id);
private:
void OnLocalSlamResult(const int trajectory_id,
const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local)
LOCKS_EXCLUDED(mutex_);
absl::Mutex mutex_;
const NodeOptions node_options_;
std::unordered_map<int,
std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
local_slam_data_ GUARDED_BY(mutex_);
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
tf2_ros::Buffer* const tf_buffer_;
std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/metrics/family_factory.h
+61
View File
@@ -0,0 +1,61 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
#include <memory>
#include <string>
#include "cartographer/metrics/family_factory.h"
#include "cartographer_ros/metrics/internal/counter.h"
#include "cartographer_ros/metrics/internal/family.h"
#include "cartographer_ros/metrics/internal/gauge.h"
#include "cartographer_ros/metrics/internal/histogram.h"
#include "cartographer_ros_msgs/srv/read_metrics.hpp"
namespace cartographer_ros {
namespace metrics {
// Realizes the factory / registry interface for the metrics in libcartographer
// and provides a wrapper to collect ROS messages from the metrics it owns.
class FamilyFactory : public ::cartographer::metrics::FamilyFactory {
public:
::cartographer::metrics::Family<::cartographer::metrics::Counter>*
NewCounterFamily(const std::string& name,
const std::string& description) override;
::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
NewGaugeFamily(const std::string& name,
const std::string& description) override;
::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
NewHistogramFamily(const std::string& name, const std::string& description,
const ::cartographer::metrics::Histogram::BucketBoundaries&
boundaries) override;
void ReadMetrics(
cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const;
private:
std::vector<std::unique_ptr<CounterFamily>> counter_families_;
std::vector<std::unique_ptr<GaugeFamily>> gauge_families_;
std::vector<std::unique_ptr<HistogramFamily>> histogram_families_;
};
} // namespace metrics
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
cartographer_ros/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
#include "cartographer/metrics/counter.h"
#include "cartographer_ros/metrics/internal/gauge.h"
#include "cartographer_ros_msgs/msg/metric.hpp"
namespace cartographer_ros {
namespace metrics {
class Counter : public ::cartographer::metrics::Counter {
public:
explicit Counter(const std::map<std::string, std::string>& labels)
: gauge_(labels) {}
void Increment(const double value) override { gauge_.Increment(value); }
void Increment() override { gauge_.Increment(); }
double Value() { return gauge_.Value(); }
cartographer_ros_msgs::msg::Metric ToRosMessage() {
cartographer_ros_msgs::msg::Metric msg = gauge_.ToRosMessage();
msg.type = cartographer_ros_msgs::msg::Metric::TYPE_COUNTER;
return msg;
}
private:
Gauge gauge_;
};
} // namespace metrics
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
cartographer_ros/cartographer_ros/include/cartographer_ros/metrics/internal/family.h
+81
View File
@@ -0,0 +1,81 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
#include <memory>
#include <string>
#include "cartographer/metrics/family_factory.h"
#include "cartographer_ros/metrics/internal/counter.h"
#include "cartographer_ros/metrics/internal/gauge.h"
#include "cartographer_ros/metrics/internal/histogram.h"
#include "cartographer_ros_msgs/msg/metric_family.hpp"
namespace cartographer_ros {
namespace metrics {
class CounterFamily
: public ::cartographer::metrics::Family<::cartographer::metrics::Counter> {
public:
CounterFamily(const std::string& name, const std::string& description)
: name_(name), description_(description) {}
Counter* Add(const std::map<std::string, std::string>& labels) override;
cartographer_ros_msgs::msg::MetricFamily ToRosMessage();
private:
std::string name_;
std::string description_;
std::vector<std::unique_ptr<Counter>> wrappers_;
};
class GaugeFamily
: public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> {
public:
GaugeFamily(const std::string& name, const std::string& description)
: name_(name), description_(description) {}
Gauge* Add(const std::map<std::string, std::string>& labels) override;
cartographer_ros_msgs::msg::MetricFamily ToRosMessage();
private:
std::string name_;
std::string description_;
std::vector<std::unique_ptr<Gauge>> wrappers_;
};
class HistogramFamily : public ::cartographer::metrics::Family<
::cartographer::metrics::Histogram> {
public:
HistogramFamily(const std::string& name, const std::string& description,
const BucketBoundaries& boundaries)
: name_(name), description_(description), boundaries_(boundaries) {}
Histogram* Add(const std::map<std::string, std::string>& labels) override;
cartographer_ros_msgs::msg::MetricFamily ToRosMessage();
private:
std::string name_;
std::string description_;
std::vector<std::unique_ptr<Histogram>> wrappers_;
const BucketBoundaries boundaries_;
};
} // namespace metrics
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
cartographer_ros/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h
+80
View File
@@ -0,0 +1,80 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
#include <map>
#include <string>
#include "absl/synchronization/mutex.h"
#include "cartographer/metrics/gauge.h"
#include "cartographer_ros_msgs/msg/metric.hpp"
namespace cartographer_ros {
namespace metrics {
class Gauge : public ::cartographer::metrics::Gauge {
public:
explicit Gauge(const std::map<std::string, std::string>& labels)
: labels_(labels), value_(0.) {}
void Decrement(const double value) override { Add(-1. * value); }
void Decrement() override { Decrement(1.); }
void Increment(const double value) override { Add(value); }
void Increment() override { Increment(1.); }
void Set(double value) override {
absl::MutexLock lock(&mutex_);
value_ = value;
}
double Value() {
absl::MutexLock lock(&mutex_);
return value_;
}
cartographer_ros_msgs::msg::Metric ToRosMessage() {
cartographer_ros_msgs::msg::Metric msg;
msg.type = cartographer_ros_msgs::msg::Metric::TYPE_GAUGE;
for (const auto& label : labels_) {
cartographer_ros_msgs::msg::MetricLabel label_msg;
label_msg.key = label.first;
label_msg.value = label.second;
msg.labels.push_back(label_msg);
}
msg.value = Value();
return msg;
}
private:
void Add(const double value) {
absl::MutexLock lock(&mutex_);
value_ += value;
}
absl::Mutex mutex_;
const std::map<std::string, std::string> labels_;
double value_ GUARDED_BY(mutex_);
};
} // namespace metrics
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h
+60
View File
@@ -0,0 +1,60 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
#include <map>
#include <vector>
#include "absl/synchronization/mutex.h"
#include "cartographer/metrics/histogram.h"
#include "cartographer_ros_msgs/msg/metric.hpp"
namespace cartographer_ros {
namespace metrics {
constexpr double kInfiniteBoundary = std::numeric_limits<double>::infinity();
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
class Histogram : public ::cartographer::metrics::Histogram {
public:
explicit Histogram(const std::map<std::string, std::string>& labels,
const BucketBoundaries& bucket_boundaries);
void Observe(double value) override;
std::map<double, double> CountsByBucket();
double Sum();
double CumulativeCount();
cartographer_ros_msgs::msg::Metric ToRosMessage();
private:
absl::Mutex mutex_;
const std::map<std::string, std::string> labels_;
const BucketBoundaries bucket_boundaries_;
std::vector<double> bucket_counts_ GUARDED_BY(mutex_);
double sum_ GUARDED_BY(mutex_);
};
} // namespace metrics
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
cartographer_ros/cartographer_ros/include/cartographer_ros/msg_conversion.h
+95
View File
@@ -0,0 +1,95 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
#include "cartographer/common/time.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer_ros_msgs/msg/landmark_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 <rclcpp/rclcpp.hpp>
namespace cartographer_ros {
sensor_msgs::msg::PointCloud2 ToPointCloud2Message(
int64_t timestamp, const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& point_cloud);
geometry_msgs::msg::Transform ToGeometryMsgTransform(
const ::cartographer::transform::Rigid3d& rigid3d);
geometry_msgs::msg::Pose ToGeometryMsgPose(
const ::cartographer::transform::Rigid3d& rigid3d);
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::msg::LaserScan& msg);
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg);
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg);
::cartographer::sensor::LandmarkData ToLandmarkData(
const cartographer_ros_msgs::msg::LandmarkList& landmark_list);
::cartographer::transform::Rigid3d ToRigid3d(
const geometry_msgs::msg::TransformStamped& transform);
::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose);
Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3);
Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion);
// Converts from WGS84 (latitude, longitude, altitude) to ECEF.
Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
double altitude);
// Returns a transform that takes ECEF coordinates from nearby points to a local
// frame that has z pointing upwards.
cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude,
double longitude);
// Points to an occupancy grid message at a specific resolution from painted
// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...).
std::unique_ptr<nav_msgs::msg::OccupancyGrid> CreateOccupancyGridMsg(
const cartographer::io::PaintSubmapSlicesResult& painted_slices,
const double resolution, const std::string& frame_id,
const rclcpp::Time& time);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
cartographer_ros/cartographer_ros/include/cartographer_ros/node.h
+246
View File
@@ -0,0 +1,246 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
#include <map>
#include <memory>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer_ros/map_builder_bridge.h"
#include "cartographer_ros/metrics/family_factory.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/srv/finish_trajectory.hpp"
#include "cartographer_ros_msgs/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 <rclcpp/rclcpp.hpp>
#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/nav_sat_fix.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
namespace cartographer_ros {
// Wires up ROS topics to SLAM.
class Node {
public:
Node(const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
std::shared_ptr<tf2_ros::Buffer> tf_buffer,
rclcpp::Node::SharedPtr node,
bool collect_metrics);
~Node();
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
// Finishes all yet active trajectories.
void FinishAllTrajectories();
// Finishes a single given trajectory. Returns false if the trajectory did not
// exist or was already finished.
bool FinishTrajectory(int trajectory_id);
// Runs final optimization. All trajectories have to be finished when calling.
void RunFinalOptimization();
// Starts the first trajectory with the default topics.
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
// Returns unique SensorIds for multiple input bag files based on
// their TrajectoryOptions.
// 'SensorId::id' is the expected ROS topic name.
std::vector<
std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
ComputeDefaultSensorIdsForMultipleBags(
const std::vector<TrajectoryOptions>& bags_options) const;
// Adds a trajectory for offline processing, i.e. not listening to topics.
int AddOfflineTrajectory(
const std::set<
cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& options);
// The following functions handle adding sensor data to a trajectory.
void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
const nav_msgs::msg::Odometry::ConstSharedPtr& msg);
void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg);
void HandleLandmarkMessage(
int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg);
void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::msg::Imu::ConstSharedPtr &msg);
void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg);
void HandleMultiEchoLaserScanMessage(
int trajectory_id, const std::string& sensor_id,
const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg);
void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
// Serializes the complete Node state.
void SerializeState(const std::string& filename,
const bool include_unfinished_submaps);
// Loads a serialized SLAM state from a .pbstream file.
void LoadState(const std::string& state_filename, bool load_frozen_state);
private:
struct Subscriber {
rclcpp::SubscriptionBase::SharedPtr subscriber;
// ::ros::Subscriber::getTopic() does not necessarily return the same
// std::string
// it was given in its constructor. Since we rely on the topic name as the
// unique identifier of a subscriber, we remember it ourselves.
std::string topic;
};
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;
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();
void PublishTrajectoryNodeList();
void PublishLandmarkPosesList();
void PublishConstraintList();
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicNames(const TrajectoryOptions& options);
cartographer_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock(
int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void MaybeWarnAboutTopicMismatch();
// Helper function for service handlers that need to check trajectory states.
cartographer_ros_msgs::msg::StatusResponse TrajectoryStateToStatus(
int trajectory_id,
const std::set<
cartographer::mapping::PoseGraphInterface::TrajectoryState>&
valid_states);
const NodeOptions node_options_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
absl::Mutex mutex_;
std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
std::shared_ptr<MapBuilderBridge> 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<geometry_msgs::msg::PoseStamped>::SharedPtr tracked_pose_publisher_;
::rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scan_matched_point_cloud_publisher_;
// These ros service servers need to live for the lifetime of the node.
::rclcpp::Service<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr submap_query_server_;
::rclcpp::Service<cartographer_ros_msgs::srv::TrajectoryQuery>::SharedPtr trajectory_query_server;
::rclcpp::Service<cartographer_ros_msgs::srv::StartTrajectory>::SharedPtr start_trajectory_server_;
::rclcpp::Service<cartographer_ros_msgs::srv::FinishTrajectory>::SharedPtr finish_trajectory_server_;
::rclcpp::Service<cartographer_ros_msgs::srv::WriteState>::SharedPtr write_state_server_;
::rclcpp::Service<cartographer_ros_msgs::srv::GetTrajectoryStates>::SharedPtr get_trajectory_states_server_;
::rclcpp::Service<cartographer_ros_msgs::srv::ReadMetrics>::SharedPtr read_metrics_server_;
struct TrajectorySensorSamplers {
TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
const double odometry_sampling_ratio,
const double fixed_frame_pose_sampling_ratio,
const double imu_sampling_ratio,
const double landmark_sampling_ratio)
: rangefinder_sampler(rangefinder_sampling_ratio),
odometry_sampler(odometry_sampling_ratio),
fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
imu_sampler(imu_sampling_ratio),
landmark_sampler(landmark_sampling_ratio) {}
::cartographer::common::FixedRatioSampler rangefinder_sampler;
::cartographer::common::FixedRatioSampler odometry_sampler;
::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
::cartographer::common::FixedRatioSampler imu_sampler;
::cartographer::common::FixedRatioSampler landmark_sampler;
};
// These are keyed with 'trajectory_id'.
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
std::map<int, builtin_interfaces::msg::Time> last_published_tf_stamps_;
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_;
std::unordered_set<int> trajectories_scheduled_for_finish_;
// The timer for publishing local trajectory data (i.e. pose transforms and
// range data point clouds) is a regular timer which is not triggered when
// simulation time is standing still. This prevents overflowing the transform
// listener buffer by publishing the same transforms over and over again.
::rclcpp::TimerBase::SharedPtr submap_list_timer_;
::rclcpp::TimerBase::SharedPtr 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
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/node_constants.h
+59
View File
@@ -0,0 +1,59 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
#include <string>
#include <vector>
namespace cartographer_ros {
// Default topic names; expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
constexpr char kMultiEchoLaserScanTopic[] = "echoes";
constexpr char kPointCloud2Topic[] = "points2";
constexpr char kImuTopic[] = "imu";
constexpr char kOdometryTopic[] = "odom";
constexpr char kNavSatFixTopic[] = "fix";
constexpr char kLandmarkTopic[] = "landmark";
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
constexpr char kOccupancyGridTopic[] = "map";
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kTrackedPoseTopic[] = "tracked_pose";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
constexpr char kReadMetricsServiceName[] = "read_metrics";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
constexpr char kConstraintListTopic[] = "constraint_list";
constexpr double kConstraintPublishPeriodSec = 0.5;
constexpr double kTopicMismatchCheckDelaySec = 3.0;
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
// For multiple topics adds numbers to the topic name and returns the list.
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
int num_topics);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
cartographer_ros/cartographer_ros/include/cartographer_ros/node_options.h
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
#include <string>
#include <tuple>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer_ros/trajectory_options.h"
namespace cartographer_ros {
// Top-level options of Cartographer's ROS integration.
struct NodeOptions {
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
std::string map_frame;
double lookup_transform_timeout_sec;
double submap_publish_period_sec;
double pose_publish_period_sec;
double trajectory_publish_period_sec;
bool publish_to_tf = true;
bool publish_tracked_pose = false;
bool use_pose_extrapolator = true;
};
NodeOptions CreateNodeOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
cartographer_ros/cartographer_ros/include/cartographer_ros/offline_node.h
+40
View File
@@ -0,0 +1,40 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer_ros/node_options.h"
namespace cartographer_ros {
using MapBuilderFactory =
std::function<std::unique_ptr<::cartographer::mapping::MapBuilderInterface>(
const ::cartographer::mapping::proto::MapBuilderOptions&)>;
void RunOfflineNode(const MapBuilderFactory& map_builder_factory,
rclcpp::Node::SharedPtr cartographer_offline_node);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/playable_bag.h
+116
View File
@@ -0,0 +1,116 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
#include <functional>
#include <queue>
#include "cartographer_ros_msgs/msg/bagfile_progress.hpp"
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/readers/sequential_reader.hpp>
#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include "tf2_ros/buffer.h"
namespace cartographer_ros {
class PlayableBag {
public:
// Handles messages early, i.e. when they are about to enter the buffer.
// Returns a boolean indicating whether the message should enter the buffer.
using FilteringEarlyMessageHandler =
std::function<bool /* forward_message_to_buffer */ (
std::shared_ptr<rosbag2_storage::SerializedBagMessage>)>;
PlayableBag(const std::string& bag_filename, int bag_id,
rclcpp::Duration buffer_delay,
FilteringEarlyMessageHandler filtering_early_message_handler);
rclcpp::Time PeekMessageTime() const;
rosbag2_storage::SerializedBagMessage GetNextMessage(
cartographer_ros_msgs::msg::BagfileProgress* progress);
bool IsMessageAvailable() const;
std::tuple<rclcpp::Time, rclcpp::Time> GetBeginEndTime() const;
int bag_id() const;
std::set<std::string> 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<rosbag2_cpp::Reader> bag_reader_;
bool finished_;
const int bag_id_;
const std::string bag_filename_;
double duration_in_seconds_;
int message_counter_;
std::deque<rosbag2_storage::SerializedBagMessage> buffered_messages_;
const rclcpp::Duration buffer_delay_;
FilteringEarlyMessageHandler filtering_early_message_handler_;
std::set<std::string> topics_;
};
class PlayableBagMultiplexer {
public:
PlayableBagMultiplexer(rclcpp::Node::SharedPtr node);
void AddPlayableBag(PlayableBag playable_bag);
// Returns the next message from the multiplexed (merge-sorted) message
// stream, along with the bag id corresponding to the message, and whether
// this was the last message in that bag.
std::tuple<rosbag2_storage::SerializedBagMessage, int, std::string, bool> GetNextMessage();
bool IsMessageAvailable() const;
rclcpp::Time PeekMessageTime() const;
std::set<std::string> topics() const { return topics_; }
private:
struct BagMessageItem {
rclcpp::Time message_timestamp;
int bag_index;
struct TimestampIsGreater {
bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
return l.message_timestamp > r.message_timestamp;
}
};
};
rclcpp::Node::SharedPtr node_;
// Publishes information about the bag-file(s) processing and its progress
rclcpp::Publisher<cartographer_ros_msgs::msg::BagfileProgress>::SharedPtr bag_progress_pub_;
// Map between bagfile id and the last time when its progress was published
std::map<int, rclcpp::Time> bag_progress_time_map_;
// The time interval of publishing bag-file(s) processing in seconds
double progress_pub_interval_;
std::vector<PlayableBag> playable_bags_;
std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
BagMessageItem::TimestampIsGreater>
next_message_queue_;
std::set<std::string> topics_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
cartographer_ros/cartographer_ros/include/cartographer_ros/ros_log_sink.h
+47
View File
@@ -0,0 +1,47 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
#include <ctime>
#include "glog/logging.h"
#include <rclcpp/rclcpp.hpp>
namespace cartographer_ros {
// Makes Google logging use ROS logging for output while an instance of this
// class exists.
class ScopedRosLogSink : public ::google::LogSink {
public:
ScopedRosLogSink();
~ScopedRosLogSink() override;
void send(::google::LogSeverity severity, const char* filename,
const char* base_filename, int line, const struct std::tm* tm_time,
const char* message, size_t message_len) override;
void WaitTillSent() override;
private:
bool will_die_;
rclcpp::Logger logger_{rclcpp::get_logger("cartographer logger")};
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
cartographer_ros/cartographer_ros/include/cartographer_ros/ros_map.h
+41
View File
@@ -0,0 +1,41 @@
/*
* Copyright 2017 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
#include <string>
#include "Eigen/Core"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/image.h"
#include "cartographer/mapping/2d/map_limits.h"
namespace cartographer_ros {
// Write 'image' as a pgm into 'file_writer'. The resolution is used in the
// comment only'
void WritePgm(const ::cartographer::io::Image& image, const double resolution,
::cartographer::io::FileWriter* file_writer);
// Write the corresponding yaml into 'file_writer'.
void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
const std::string& pgm_filename,
::cartographer::io::FileWriter* file_writer);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
cartographer_ros/cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#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/probability_grid_range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
namespace cartographer_ros {
// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes
// out a PGM and YAML suitable for ROS map server to consume.
class RosMapWritingPointsProcessor
: public ::cartographer::io::PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "write_ros_map";
RosMapWritingPointsProcessor(
double resolution,
const ::cartographer::mapping::proto::
ProbabilityGridRangeDataInserterOptions2D&
range_data_inserter_options,
::cartographer::io::FileWriterFactory file_writer_factory,
const std::string& filestem, PointsProcessor* next);
RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
delete;
static std::unique_ptr<RosMapWritingPointsProcessor> FromDictionary(
::cartographer::io::FileWriterFactory file_writer_factory,
::cartographer::common::LuaParameterDictionary* dictionary,
PointsProcessor* next);
~RosMapWritingPointsProcessor() override {}
void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override;
FlushResult Flush() override;
private:
const std::string filestem_;
PointsProcessor* const next_;
::cartographer::io::FileWriterFactory file_writer_factory_;
::cartographer::mapping::ProbabilityGridRangeDataInserter2D
range_data_inserter_;
::cartographer::mapping::ValueConversionTables conversion_tables_;
::cartographer::mapping::ProbabilityGrid probability_grid_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
cartographer_ros/cartographer_ros/include/cartographer_ros/sensor_bridge.h
+99
View File
@@ -0,0 +1,99 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
#include <memory>
#include "absl/types/optional.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros_msgs/msg/landmark_list.hpp"
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#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/nav_sat_fix.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
namespace cartographer_ros {
// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
class SensorBridge {
public:
explicit SensorBridge(
int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::msg::Odometry::ConstSharedPtr& msg);
void HandleOdometryMessage(const std::string& sensor_id,
const nav_msgs::msg::Odometry::ConstSharedPtr& msg);
void HandleNavSatFixMessage(const std::string& sensor_id,
const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg);
void HandleLandmarkMessage(
const std::string& sensor_id,
const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg);
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::msg::Imu::ConstSharedPtr& msg);
void HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::msg::Imu::ConstSharedPtr& msg);
void HandleLaserScanMessage(const std::string& sensor_id,
const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg);
void HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg);
void HandlePointCloud2Message(const std::string& sensor_id,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
const TfBridge& tf_bridge() const;
private:
void HandleLaserScan(
const std::string& sensor_id, ::cartographer::common::Time start_time,
const std::string& frame_id,
const ::cartographer::sensor::PointCloudWithIntensities& points);
void HandleRangefinder(const std::string& sensor_id,
::cartographer::common::Time time,
const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/submap.h
+43
View File
@@ -0,0 +1,43 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
#include <memory>
#include <string>
#include <vector>
#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_msgs/srv/submap_query.hpp"
#include <rclcpp/rclcpp.hpp>
namespace cartographer_ros {
// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
// on error.
std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id,
rclcpp::Client<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr client,
rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor,
const std::chrono::milliseconds timeout);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
cartographer_ros/cartographer_ros/include/cartographer_ros/tf_bridge.h
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
#include <memory>
#include "cartographer/transform/rigid_transform.h"
#include "cartographer_ros/time_conversion.h"
#include <tf2_ros/buffer.h>
#include <rclcpp/rclcpp.hpp>
namespace cartographer_ros {
class TfBridge {
public:
TfBridge(const std::string& tracking_frame,
double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
~TfBridge() {}
TfBridge(const TfBridge&) = delete;
TfBridge& operator=(const TfBridge&) = delete;
// Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time'.
std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
::cartographer::common::Time time, const std::string& frame_id) const;
private:
const std::string tracking_frame_;
const double lookup_transform_timeout_sec_;
const tf2_ros::Buffer* const buffer_;
};
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
cartographer_ros/cartographer_ros/include/cartographer_ros/time_conversion.h
+32
View File
@@ -0,0 +1,32 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
#include "cartographer/common/time.h"
#include <builtin_interfaces/msg/time.hpp>
#include <rclcpp/rclcpp.hpp>
namespace cartographer_ros {
rclcpp::Time ToRos(::cartographer::common::Time time);
::cartographer::common::Time FromRos(const rclcpp::Time& time);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
cartographer_ros/cartographer_ros/include/cartographer_ros/trajectory_options.h
+55
View File
@@ -0,0 +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.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
namespace cartographer_ros {
struct TrajectoryOptions {
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options;
std::string tracking_frame;
std::string published_frame;
std::string odom_frame;
bool provide_odom_frame;
bool use_odometry;
bool use_nav_sat;
bool use_landmarks;
bool publish_frame_projected_to_2d;
int num_laser_scans;
int num_multi_echo_laser_scans;
int num_subdivisions_per_laser_scan;
int num_point_clouds;
double rangefinder_sampling_ratio;
double odometry_sampling_ratio;
double fixed_frame_pose_sampling_ratio;
double imu_sampling_ratio;
double landmarks_sampling_ratio;
};
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
cartographer_ros/cartographer_ros/include/cartographer_ros/urdf_reader.h
+32
View File
@@ -0,0 +1,32 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
#include <vector>
#include "cartographer/common/port.h"
#include "tf2_ros/buffer.h"
namespace cartographer_ros {
std::vector<geometry_msgs::msg::TransformStamped> ReadStaticTransformsFromUrdf(
const std::string& urdf_filename, std::shared_ptr<tf2_ros::Buffer> tf_buffer);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
cartographer_ros/cartographer_ros/launch/agvpro_backpack_2d.launch.py
+67
View File
@@ -0,0 +1,67 @@
"""
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.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 *****
use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')
## ***** File paths ******
pkg_share = FindPackageShare('agv_pro_description').find('agv_pro_description')
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', 'agvpro_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': False},
{'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': False}],
)
return LaunchDescription([
use_sim_time_arg,
# Nodes
rviz_node,
cartographer_node,
cartographer_occupancy_grid_node,
])
cartographer_ros/cartographer_ros/launch/assets_writer_backpack_2d.launch.py
+58
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/assets_writer_backpack_3d.launch.py
+58
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/assets_writer_ros_map.launch.py
+54
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/backpack_2d.launch.py
+75
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/backpack_3d.launch.py
+76
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/demo_backpack_2d.launch.py
+57
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/demo_backpack_2d_localization.launch.py
+94
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/demo_backpack_3d.launch.py
+57
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/demo_backpack_3d_localization.launch.py
+95
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/demo_pr2.launch
+47
View File
@@ -0,0 +1,47 @@
<!--
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.
-->
<launch>
<param name="/use_sim_time" value="true" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory
$(find cartographer_ros)/configuration_files
-configuration_basename pr2.lua"
output="screen">
<remap from="scan" to="/base_scan" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="tf_remove_frames" pkg="cartographer_ros"
type="tf_remove_frames.py">
<remap from="tf_out" to="/tf" />
<rosparam param="remove_frames">
- map
- odom_combined
</rosparam>
</node>
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)">
<remap from="tf" to="tf_in" />
</node>
</launch>
cartographer_ros/cartographer_ros/launch/demo_revo_lds.launch.py
+91
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/demo_taurob_tracker.launch.py
+57
View File
@@ -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
])
cartographer_ros/cartographer_ros/launch/grpc_demo_backpack_2d.launch
+45
View File
@@ -0,0 +1,45 @@
<!--
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.
-->
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_grpc_server" pkg="cartographer_ros"
type="cartographer_grpc_server.sh" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_server.lua">
</node>
<node name="cartographer_grpc_node" pkg="cartographer_ros"
type="cartographer_grpc_node" args="
-client_id CLIENT_ID
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
cartographer_ros/cartographer_ros/launch/grpc_demo_backpack_2d_localization.launch
+40
View File
@@ -0,0 +1,40 @@
<!--
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.
-->
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_grpc_node" pkg="cartographer_ros"
type="cartographer_grpc_node" args="
-client_id CLIENT_ID
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua
-map_filename $(arg map_filename)"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
cartographer_ros/cartographer_ros/launch/offline_backpack_2d.launch.py
+62
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/offline_backpack_3d.launch.py
+64
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/offline_mir_100_rviz.launch.py
+76
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/offline_node.launch.py
+81
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/taurob_tracker.launch.py
+58
View File
@@ -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,
])
cartographer_ros/cartographer_ros/launch/visualize_pbstream.launch.py
+57
View File
@@ -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
])
cartographer_ros/cartographer_ros/package.xml
+108
View File
@@ -0,0 +1,108 @@
<?xml version="1.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.
-->
<package format="3">
<name>cartographer_ros</name>
<!--
As of 2022-03-31, the versioning for cartographer_ros is in a complicated situation.
Upstream https://github.com/cartographer-project/cartographer_ros seems to be moribund,
with no commits for close to a year. The master branch there is only for ROS 1, and
that branch has not had a released tag since 1.0.0 in 2018.
In the meantime, we've done a port to ROS 2 in https://github.com/ros2/cartographer_ros
that targets upstream cartographer 2.0.0.
Since we have no response from upstream in a long time, we are going to go ahead and
assume that the 1.x series of tags from upstream cartographer_ros will always be used
to target ROS 1. Further we are going to assume that the 2.x series of tags
from upstream cartographer_ros (none of which exist yet) will be used to target ROS 2.
If upstream comes alive and chooses a different versioning scheme, we can adjust later
as necessary. The outcome of all of this is that we are assuming a starting "upstream"
release of 2.0.0, which then plays into the rest of the explanation below.
We add 900 to the patch part of the version and then multiply it by 10,
i.e. our version = `(upstream_patch_version + 900) * 10`,
so we can have intermediate releases as well as release any future official 2.0.x versions.
This is basically packing the patch part of the version and a fourth version part together
into the third part of the version.
The use of `900` instead of something else like `100` is arbitrary, but it might
help people recognize that this is a "special" version number.
It is needed however, because we cannot have a leading `0` in our patch version.
Consider these possible future versions as an example:
2.0.9000 -> current state of this repository, 2.0.0 + some commits from us
2.0.9010 -> upstream 2.0.1
2.0.9011 -> upstream 2.0.1 + additional commits from upstream or us
2.0.9012 -> upstream 2.0.1 + additional commits from 2.0.1011 + more new commits
2.0.9020 -> upstream 2.0.2
and so on...
-->
<version>2.0.9003</version>
<description>
Cartographer is a system that provides real-time simultaneous localization
and mapping (SLAM) in 2D and 3D across multiple platforms and sensor
configurations. This package provides Cartographer's ROS integration.
</description>
<maintainer email="clalancette@openrobotics.org">Chris Lalancette</maintainer>
<license>Apache 2.0</license>
<url>https://github.com/cartographer-project/cartographer_ros</url>
<author email="google-cartographer@googlegroups.com">
The Cartographer Authors
</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>google-mock</build_depend>
<build_depend>python3-sphinx</build_depend>
<build_depend>ros_environment</build_depend>
<depend>builtin_interfaces</depend>
<depend>cartographer</depend>
<depend>cartographer_ros_msgs</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>libabsl-dev</depend>
<depend>libcairo2-dev</depend>
<depend>libgflags-dev</depend>
<depend>libgoogle-glog-dev</depend>
<depend>libpcl-all-dev</depend>
<depend>nav_msgs</depend>
<depend>pcl_conversions</depend>
<depend>rclcpp</depend>
<depend>rosbag2_cpp</depend>
<depend>rosbag2_storage</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<depend>urdf</depend>
<depend>visualization_msgs</depend>
<exec_depend>launch</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
cartographer_ros/cartographer_ros/scripts/cartographer_grpc_server.sh
Executable
+18
View File
@@ -0,0 +1,18 @@
#!/bin/bash
# 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 -xe
cartographer_grpc_server $@
cartographer_ros/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh
Executable
+74
View File
@@ -0,0 +1,74 @@
#!/bin/bash
# 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
MAPFILE="$1"
BAGFILE="$2"
if [ "$#" -ne 2 ]; then
echo "\n\nUsage: $0 <frozen_map_file> <bag_file>\n"
exit 1;
fi
LAUNCHSCRIPT='
<launch>
<node name="cartographer_offline_node" pkg="cartographer_ros"
required="true"
type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basenames backpack_2d.lua
-urdf_filenames $(find cartographer_ros)/urdf/backpack_2d.urdf
-load_state_filename '$MAPFILE'
-bag_filenames '$BAGFILE'
">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
</launch>'
echo $LAUNCHSCRIPT | roslaunch -
LAUNCHSCRIPT='
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
required="true"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization_evaluation.lua
-load_state_filename '$MAPFILE'
-load_frozen_state true
">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="playbag" pkg="rosbag" type="play"
required="true"
args="--clock '$BAGFILE'" />
<node name="recordtf" pkg="rosbag" type="record"
args="
tf
-O '$BAGFILE'.tf-result
" />
</launch>
'
echo $LAUNCHSCRIPT | roslaunch -
rosrun cartographer_ros cartographer_dev_trajectory_comparison \
-bag_filename $BAGFILE.tf-result.bag \
-pbstream_filename $BAGFILE.pbstream
cartographer_ros/cartographer_ros/scripts/dev/publish_fake_random_landmarks.py
Executable
+191
View File
@@ -0,0 +1,191 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2018 Magazino GmbH
# The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import itertools
import random
import rospy
from tf import transformations
from cartographer_ros_msgs.msg import LandmarkEntry, LandmarkList
DESC = '''
Samples a number of random landmarks at a specified rate.
Can be used to test the timing of landmark input or the effect of erroneous
landmarks with duplicate IDs (if --allow_duplicate_ids is set).
For example:
./publish_fake_random_landmarks.py \\
--publish_period 0.1 \\
--id_vocabulary A B C \\
--id_length 5 \\
--sample_period 1.0
will publish empty landmark lists at 10Hz and random landmarks every second.
IDs are also sampled, using the cartesian product of the provided "vocabulary".
In the above example, a sampled ID could be e.g. "AACBC" (length=5).
'''
TOPIC = "landmark"
class LandmarkSamplerOptions(object):
def __init__(self, *args, **kwargs):
self.allow_duplicate_ids = False
self.id_vocabulary = {}
self.id_length = 0
self.max_distance = 0.
self.num_landmarks = 0
self.rotation_weight = 0.
self.translation_weight = 0.
for name, arg in kwargs.items():
setattr(self, name, arg)
class LandmarkIdSampler(object):
def __init__(self, id_vocabulary, id_length):
# Precompute all combinations of the symbols in the vocabulary.
# WARNING: can be huge, check before potentially blocking the system.
if len(id_vocabulary)**id_length > 1e6:
raise ValueError("ID sampling space is too large")
self.sampling_space = list(
itertools.product(*(id_vocabulary for i in range(id_length))))
def sample_id(self):
# Draw a random combination of symbols and stringify it.
random_index = random.randint(0, len(self.sampling_space) - 1)
sampled_id = "".join(self.sampling_space[random_index])
return sampled_id
class LandmarkSampler(object):
def __init__(self, options):
if not isinstance(options, LandmarkSamplerOptions):
raise TypeError("expected LandmarkSamplerOptions")
self.options = options
rospy.loginfo("Initializing landmark ID sampler...")
self.landmark_id_sampler = LandmarkIdSampler(options.id_vocabulary,
options.id_length)
self._sampled_ids = []
def random_landmark(self):
landmark = LandmarkEntry()
landmark.translation_weight = self.options.translation_weight
landmark.rotation_weight = self.options.rotation_weight
landmark.id = self.landmark_id_sampler.sample_id()
if landmark.id in self._sampled_ids:
if not self.options.allow_duplicate_ids:
rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id))
return None
else:
rospy.logwarn("Duplicate ID: {}".format(landmark.id))
self._sampled_ids.append(landmark.id)
vector = transformations.random_vector(3) * self.options.max_distance
landmark.tracking_from_landmark_transform.position.x = vector[0]
landmark.tracking_from_landmark_transform.position.y = vector[1]
landmark.tracking_from_landmark_transform.position.z = vector[2]
quaternion = transformations.random_quaternion()
landmark.tracking_from_landmark_transform.orientation.x = quaternion[0]
landmark.tracking_from_landmark_transform.orientation.y = quaternion[1]
landmark.tracking_from_landmark_transform.orientation.z = quaternion[2]
landmark.tracking_from_landmark_transform.orientation.w = quaternion[3]
return landmark
def random_landmark_list(self):
landmark_list = LandmarkList()
landmark_list.header.stamp = rospy.Time.now()
for _ in range(self.options.num_landmarks):
random_landmark = self.random_landmark()
if random_landmark is not None:
landmark_list.landmarks.append(random_landmark)
return landmark_list
class SampledLandmarkPublisher(object):
def __init__(self, publish_period, sample_period, landmark_sampler_options):
self.landmark_sampler = LandmarkSampler(landmark_sampler_options)
rospy.loginfo("Publishing landmarks to topic: {}".format(TOPIC))
self.publisher = rospy.Publisher(TOPIC, LandmarkList, queue_size=1)
self.publish_timer = rospy.Timer(
rospy.Duration(publish_period), self.publish_empty_landmark_list)
self.sample_timer = rospy.Timer(
rospy.Duration(sample_period), self.publish_random_landmark_list)
def publish_random_landmark_list(self, timer_event):
self.publisher.publish(self.landmark_sampler.random_landmark_list())
def publish_empty_landmark_list(self, timer_event):
landmark_list = LandmarkList(rospy.Header(stamp=rospy.Time.now()), [])
self.publisher.publish(landmark_list)
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser(
description=DESC, formatter_class=argparse.RawTextHelpFormatter)
parser.add_argument("--translation_weight", type=float, default=1e5)
parser.add_argument("--rotation_weight", type=float, default=1e5)
parser.add_argument(
"--publish_period",
type=float,
default=0.1,
help="Baseline period for publishing empty landmark lists.")
parser.add_argument(
"--sample_period",
type=float,
default=5.,
help="Period at which randomly sampled landmarks are published.")
parser.add_argument(
"--num_landmarks",
type=int,
default=5,
help="The number of random landmarks published simultaneously.")
parser.add_argument(
"--max_distance",
type=float,
default=1.0,
help="Maximum distance of a random landmark to the tracking frame.")
parser.add_argument(
"--id_vocabulary",
nargs='+',
default={"a", "b", "c", "1", "2", "3"},
help="Set of symbols that can appear in random landmark IDs.")
parser.add_argument(
"--id_length",
type=int,
default=5,
help="The length of the random landmark IDs (number of symbols).")
parser.add_argument(
"--allow_duplicate_ids",
action="store_true",
help="Publish landmarks with IDs that have already been published.")
args, unknown = parser.parse_known_args(rospy.myargv()[1:])
rospy.init_node("landmark_sampler")
landmark_sampler_options = LandmarkSamplerOptions(**args.__dict__)
sampler = SampledLandmarkPublisher(args.publish_period, args.sample_period,
landmark_sampler_options)
rospy.spin()
cartographer_ros/cartographer_ros/scripts/remove_leading_slashes.py
Executable
+53
View File
@@ -0,0 +1,53 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
# 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.
"""A simple tool to remove leading slashes from frame names."""
import argparse
import os
import rosbag
def ParseArgs():
argument_parser = argparse.ArgumentParser(
description="Removes leading slashes from frame names.")
argument_parser.add_argument("input", type=str, help="Input bag")
return argument_parser.parse_args()
def RewriteMsg(msg):
if hasattr(msg, "header"):
if msg.header.frame_id.startswith("/"):
msg.header.frame_id = msg.header.frame_id[1:]
if hasattr(msg, "child_frame_id"):
if msg.child_frame_id.startswith("/"):
msg.child_frame_id = msg.child_frame_id[1:]
if hasattr(msg, "transforms"):
for transform_msg in msg.transforms:
RewriteMsg(transform_msg)
def Main():
options = ParseArgs()
with rosbag.Bag(os.path.splitext(options.input)[0] + ".filtered.bag",
"w") as outbag:
for topic, msg, t in rosbag.Bag(options.input).read_messages():
RewriteMsg(msg)
outbag.write(topic, msg, msg.header.stamp if msg._has_header else t)
if __name__ == "__main__":
Main()
cartographer_ros/cartographer_ros/scripts/tf_remove_frames.py
Executable
+40
View File
@@ -0,0 +1,40 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
# 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.
import rospy
from tf.msg import tfMessage
def main():
rospy.init_node('tf_remove_frames')
publisher = rospy.Publisher('/tf_out', tfMessage, queue_size=1)
remove_frames = rospy.get_param('~remove_frames', [])
def callback(msg):
msg.transforms = [
t for t in msg.transforms
if t.header.frame_id.lstrip('/') not in remove_frames and
t.child_frame_id.lstrip('/') not in remove_frames
]
publisher.publish(msg)
rospy.Subscriber('/tf_in', tfMessage, callback)
rospy.spin()
if __name__ == '__main__':
main()
cartographer_ros/cartographer_ros/src/.clang-format
+2
View File
@@ -0,0 +1,2 @@
BasedOnStyle: Google
DerivePointerAlignment: false
cartographer_ros/cartographer_ros/src/assets_writer.cpp
+310
View File
@@ -0,0 +1,310 @@
/*
* 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/assets_writer.h"
#include <algorithm>
#include <fstream>
#include <iostream>
#include "absl/memory/memory.h"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/math.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/io/points_processor_pipeline_builder.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/ros_map_writing_points_processor.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/urdf_reader.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "builtin_interfaces/msg/time.hpp"
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/readers/sequential_reader.hpp>
#include "tf2_eigen/tf2_eigen.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_ros/buffer.h"
#ifdef USE_URDF_H_FILES
#include "urdf/model.h"
#else
#include "urdf/model.hpp"
#endif
namespace cartographer_ros {
namespace {
constexpr char kTfStaticTopic[] = "/tf_static";
namespace carto = ::cartographer;
std::unique_ptr<carto::io::PointsProcessorPipelineBuilder>
CreatePipelineBuilder(
const std::vector<carto::mapping::proto::Trajectory>& trajectories,
const std::string file_prefix) {
const auto file_writer_factory =
AssetsWriter::CreateFileWriterFactory(file_prefix);
auto builder = absl::make_unique<carto::io::PointsProcessorPipelineBuilder>();
carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory,
builder.get());
builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName,
[file_writer_factory](
carto::common::LuaParameterDictionary* const dictionary,
carto::io::PointsProcessor* const next)
-> std::unique_ptr<carto::io::PointsProcessor> {
return RosMapWritingPointsProcessor::FromDictionary(
file_writer_factory, dictionary, next);
});
return builder;
}
std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
const std::string& configuration_directory,
const std::string& configuration_basename) {
auto file_resolver =
absl::make_unique<carto::common::ConfigurationFileResolver>(
std::vector<std::string>{configuration_directory});
const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename);
auto lua_parameter_dictionary =
absl::make_unique<carto::common::LuaParameterDictionary>(
code, std::move(file_resolver));
return lua_parameter_dictionary;
}
template <typename T>
std::unique_ptr<carto::io::PointsBatch> HandleMessage(
const T& message, const std::string& tracking_frame,
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
const carto::transform::TransformInterpolationBuffer&
transform_interpolation_buffer) {
const carto::common::Time start_time = FromRos(message->header.stamp);
auto points_batch = absl::make_unique<carto::io::PointsBatch>();
points_batch->start_time = start_time;
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);
CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size());
for (size_t i = 0; i < point_cloud.points.size(); ++i) {
const carto::common::Time time =
point_cloud_time +
carto::common::FromSeconds(point_cloud.points[i].time);
if (!transform_interpolation_buffer.Has(time)) {
continue;
}
const carto::transform::Rigid3d tracking_to_map =
transform_interpolation_buffer.Lookup(time);
const carto::transform::Rigid3d sensor_to_tracking =
ToRigid3d(tf_buffer->lookupTransform(
tracking_frame, message->header.frame_id, ToRos(time)));
const carto::transform::Rigid3f sensor_to_map =
(tracking_to_map * sensor_to_tracking).cast<float>();
points_batch->points.push_back(
sensor_to_map *
carto::sensor::ToRangefinderPoint(point_cloud.points[i]));
points_batch->intensities.push_back(point_cloud.intensities[i]);
// We use the last transform for the origin, which is approximately correct.
points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
}
if (points_batch->points.empty()) {
return nullptr;
}
return points_batch;
}
} // namespace
AssetsWriter::AssetsWriter(const std::string& pose_graph_filename,
const std::vector<std::string>& bag_filenames,
const std::string& output_file_prefix)
: bag_filenames_(bag_filenames),
pose_graph_(
carto::io::DeserializePoseGraphFromFile(pose_graph_filename)) {
CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size())
<< "Pose graphs contains " << pose_graph_.trajectory_size()
<< " trajectories while " << bag_filenames_.size()
<< " bags were provided. This tool requires one bag for each "
"trajectory in the same order as the correponding trajectories in the "
"pose graph proto.";
// This vector must outlive the pipeline.
all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>(
pose_graph_.trajectory().begin(), pose_graph_.trajectory().end());
const std::string file_prefix = !output_file_prefix.empty()
? output_file_prefix
: bag_filenames_.front() + "_";
point_pipeline_builder_ =
CreatePipelineBuilder(all_trajectories_, file_prefix);
}
void AssetsWriter::RegisterPointsProcessor(
const std::string& name,
cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction factory) {
point_pipeline_builder_->Register(name, factory);
}
void AssetsWriter::Run(const std::string& configuration_directory,
const std::string& configuration_basename,
const std::string& urdf_filename,
const bool use_bag_transforms) {
const auto lua_parameter_dictionary =
LoadLuaDictionary(configuration_directory, configuration_basename);
std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
point_pipeline_builder_->CreatePipeline(
lua_parameter_dictionary->GetDictionary("pipeline").get());
const std::string tracking_frame =
lua_parameter_dictionary->GetString("tracking_frame");
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
do {
for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size();
++trajectory_id) {
const carto::mapping::proto::Trajectory& trajectory_proto =
pose_graph_.trajectory(trajectory_id);
const std::string& bag_filename = bag_filenames_[trajectory_id];
LOG(INFO) << "Processing " << bag_filename << "...";
if (trajectory_proto.node_size() == 0) {
continue;
}
std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>(clock);
tf_buffer->setUsingDedicatedThread(true);
if (!urdf_filename.empty()) {
ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer);
}
const carto::transform::TransformInterpolationBuffer
transform_interpolation_buffer(trajectory_proto);
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<rosbag2_storage::SerializedBagMessage> 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 rclcpp::Duration kDelay(1.0,0.0);
auto serializer = rclcpp::Serialization<tf2_msgs::msg::TFMessage>();
auto laser_scan_serializer = rclcpp::Serialization<sensor_msgs::msg::LaserScan>();
auto multi_echo_laser_scan_serializer = rclcpp::Serialization<sensor_msgs::msg::MultiEchoLaserScan>();
auto pcl2_serializer = rclcpp::Serialization<sensor_msgs::msg::PointCloud2>();
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->topic_name == kTfStaticTopic);
} catch (const tf2::TransformException& ex) {
LOG(WARNING) << ex.what();
}
}
}
#ifdef PRE_JAZZY_SERIALIZED_BAG_MSG_FIELD_NAME
while (!delayed_messages.empty() && delayed_messages.front().time_stamp <
message->time_stamp - kDelay.nanoseconds()) {
#else
while (!delayed_messages.empty() && delayed_messages.front().recv_timestamp <
message->recv_timestamp - kDelay.nanoseconds()) {
#endif
const auto delayed_message =
delayed_messages.front();
std::unique_ptr<carto::io::PointsBatch> 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<sensor_msgs::msg::LaserScan>();
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<sensor_msgs::msg::MultiEchoLaserScan>();
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<sensor_msgs::msg::PointCloud2>();
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.push_back(*message);
#ifdef PRE_JAZZY_SERIALIZED_BAG_MSG_FIELD_NAME
LOG_EVERY_N(INFO, 10000)
<< "Processed " << (message->time_stamp - begin_time.nanoseconds())/1e9
<< " of " << bag_metadata.duration.count()/1e9 << " bag time seconds...";
#else
LOG_EVERY_N(INFO, 10000)
<< "Processed " << (message->recv_timestamp - begin_time.nanoseconds())/1e9
<< " of " << bag_metadata.duration.count()/1e9 << " bag time seconds...";
#endif
}
}
} while (pipeline.back()->Flush() ==
carto::io::PointsProcessor::FlushResult::kRestartStream);
}
::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory(
const std::string& file_path) {
const auto file_writer_factory = [file_path](const std::string& filename) {
return absl::make_unique<carto::io::StreamFileWriter>(file_path + filename);
};
return file_writer_factory;
}
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/assets_writer_main.cpp
+78
View File
@@ -0,0 +1,78 @@
/*
* 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/assets_writer.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "rclcpp/rclcpp.hpp"
#include <regex>
#include <string>
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(
urdf_filename, "",
"URDF file that contains static links for your sensor configuration.");
DEFINE_string(bag_filenames, "",
"Bags to process, must be in the same order as the trajectories "
"in 'pose_graph_filename'.");
DEFINE_string(pose_graph_filename, "",
"Proto stream file containing the pose graph.");
DEFINE_bool(use_bag_transforms, true,
"Whether to read and use the transforms from the bag.");
DEFINE_string(output_file_prefix, "",
"Will be prefixed to all output file names and can be used to "
"define the output directory. If empty, the first bag filename "
"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, false);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
CHECK(!FLAGS_pose_graph_filename.empty())
<< "-pose_graph_filename is missing.";
std::regex regex(",");
std::vector<std::string> 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,
bag_filenames,
FLAGS_output_file_prefix);
asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename,
FLAGS_urdf_filename, FLAGS_use_bag_transforms);
rclcpp::shutdown();
}
cartographer_ros/cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp
+117
View File
@@ -0,0 +1,117 @@
/*
* Copyright 2017 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 "absl/memory/memory.h"
#include "cartographer/cloud/client/map_builder_stub.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
DEFINE_bool(collect_metrics, false,
"Activates the collection of runtime metrics. If activated, the "
"metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(server_address, "localhost:50051",
"gRPC server address to stream the sensor data to.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_map_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file "
"to load, containing a saved SLAM state. "
"Unless --upload_load_state_file is set, the filepath refers "
"to the gRPC server's file system.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(upload_load_state_file, false,
"Upload the .pbstream file from a local path to the (remote) gRPC "
"server instead of loading it from the server file system.");
DEFINE_string(client_id, "",
"Cartographer client ID to use when connecting to the server.");
namespace cartographer_ros {
namespace {
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(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<::cartographer::cloud::MapBuilderStub>(
FLAGS_server_address, FLAGS_client_id);
if (!FLAGS_load_state_filename.empty() && !FLAGS_upload_load_state_file) {
map_builder->LoadStateFromFile(FLAGS_load_state_filename,
FLAGS_load_frozen_state);
}
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
if (!FLAGS_load_state_filename.empty() && FLAGS_upload_load_state_file) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
node.FinishAllTrajectories();
node.RunFinalOptimization();
if (!FLAGS_save_map_filename.empty()) {
node.SerializeState(FLAGS_save_map_filename,
false /* include_unfinished_submaps */);
}
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
::ros::init(argc, argv, "cartographer_grpc_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}
cartographer_ros/cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp
+49
View File
@@ -0,0 +1,49 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/cloud/client/map_builder_stub.h"
#include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "ros/ros.h"
DEFINE_string(server_address, "localhost:50051",
"gRPC server address to "
"stream the sensor data to.");
DEFINE_string(client_id, "",
"Cartographer client ID to use when connecting to the server.");
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
::ros::init(argc, argv, "cartographer_grpc_offline_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
const cartographer_ros::MapBuilderFactory map_builder_factory =
[](const ::cartographer::mapping::proto::MapBuilderOptions&) {
return absl::make_unique< ::cartographer::cloud::MapBuilderStub>(
FLAGS_server_address, FLAGS_client_id);
};
cartographer_ros::RunOfflineNode(map_builder_factory);
::ros::shutdown();
}
cartographer_ros/cartographer_ros/src/configuration_files_test.cpp
+44
View File
@@ -0,0 +1,44 @@
/*
* 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 <string>
#include <vector>
#include "cartographer_ros/node_options.h"
#include "gtest/gtest.h"
#include "ros/package.h"
namespace cartographer_ros {
namespace {
class ConfigurationFilesTest : public ::testing::TestWithParam<const char*> {};
TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
EXPECT_NO_FATAL_FAILURE({
LoadOptions(
::ros::package::getPath("cartographer_ros") + "/configuration_files",
GetParam());
});
}
INSTANTIATE_TEST_CASE_P(
ValidateAllNodeOptions, ConfigurationFilesTest,
::testing::Values("backpack_2d.lua", "backpack_2d_localization.lua",
"backpack_3d.lua", "backpack_3d_localization.lua",
"pr2.lua", "revo_lds.lua", "taurob_tracker.lua"));
} // namespace
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp
+102
View File
@@ -0,0 +1,102 @@
/*
* Copyright 2019 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <iostream>
#include <string>
#include <vector>
#include "absl/strings/str_cat.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "geometry_msgs/TransformStamped.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "rosbag/bag.h"
#include "tf2_msgs/TFMessage.h"
DEFINE_string(input, "", "pbstream file to process");
DEFINE_string(output, "", "Bag file to write to.");
DEFINE_string(parent_frame, "map", "Frame id to use as parent frame.");
namespace cartographer_ros {
namespace {
geometry_msgs::msg::TransformStamped ToTransformStamped(
int64_t timestamp_uts, const std::string& parent_frame_id,
const std::string& child_frame_id,
const cartographer::transform::proto::Rigid3d& parent_T_child) {
static int64_t seq = 0;
geometry_msgs::msg::TransformStamped transform_stamped;
transform_stamped.header.seq = ++seq;
transform_stamped.header.frame_id = parent_frame_id;
transform_stamped.header.stamp = cartographer_ros::ToRos(
::cartographer::common::FromUniversal(timestamp_uts));
transform_stamped.child_frame_id = child_frame_id;
transform_stamped.transform = cartographer_ros::ToGeometryMsgTransform(
::cartographer::transform::ToRigid3(parent_T_child));
return transform_stamped;
}
void pbstream_trajectories_to_bag(const std::string& pbstream_filename,
const std::string& output_bag_filename,
const std::string& parent_frame_id) {
const auto pose_graph =
cartographer::io::DeserializePoseGraphFromFile(FLAGS_input);
rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write);
for (const auto trajectory : pose_graph.trajectory()) {
const auto child_frame_id =
absl::StrCat("trajectory_", trajectory.trajectory_id());
LOG(INFO)
<< "Writing tf and geometry_msgs/TransformStamped for trajectory id "
<< trajectory.trajectory_id() << " with " << trajectory.node_size()
<< " nodes.";
for (const auto& node : trajectory.node()) {
tf2_msgs::TFMessage tf_msg;
geometry_msgs::msg::TransformStamped transform_stamped = ToTransformStamped(
node.timestamp(), parent_frame_id, child_frame_id, node.pose());
tf_msg.transforms.push_back(transform_stamped);
bag.write(child_frame_id, transform_stamped.header.stamp,
transform_stamped);
bag.write("/tf", transform_stamped.header.stamp, tf_msg);
}
}
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char* argv[]) {
FLAGS_alsologtostderr = true;
google::InitGoogleLogging(argv[0]);
google::SetUsageMessage(
"\n\n"
"Extracts all trajectories from the pbstream and creates a bag file with "
"the trajectory poses stored in /tf.\nAdditionally, each trajectory is "
"also written separately to a geometry_msgs/TransformStamped topic named "
"after the TF child_frame_id of the trajectory.\n For each trajectory, "
"the tool will write transforms with the tf parent_frame_id set "
"according to the `parent_frame` commandline flag and child_frame_id to "
"`trajectory_i`, with `i` corresponding to the `trajectory_id`.");
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_input.empty()) << "-input pbstream is missing.";
CHECK(!FLAGS_output.empty()) << "-output is missing.";
cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output,
FLAGS_parent_frame);
return 0;
}
cartographer_ros/cartographer_ros/src/dev/rosbag_publisher_main.cpp
+148
View File
@@ -0,0 +1,148 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/common/time.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/time_conversion.h"
#include "gflags/gflags.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "ros/time.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf2_msgs/TFMessage.h"
DEFINE_string(bag_filename, "", "Bag to publish.");
const int kQueueSize = 1;
template <typename MessagePtrType>
void PublishWithModifiedTimestamp(MessagePtrType message,
const ros::Publisher& publisher,
ros::Duration bag_to_current) {
rclcpp::Time& stamp = message->header.stamp;
stamp += bag_to_current;
publisher.publish(message);
}
template <>
void PublishWithModifiedTimestamp<tf2_msgs::TFMessage::Ptr>(
tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher,
ros::Duration bag_to_current) {
for (const auto& transform : message->transforms) {
rclcpp::Time& stamp = const_cast<rclcpp::Time&>(transform.header.stamp);
stamp += bag_to_current;
}
publisher.publish(message);
}
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::SetUsageMessage(
"\n\n"
"This replays and publishes messages from a given bag file, modifying "
"their header timestamps to match current ROS time.\n\n"
"Messages are published in the same sequence and with the same delay "
"they were recorded."
"Contrary to rosbag play, it does not publish a clock, so time is"
"hopefully smoother and it should be possible to reproduce timing"
"issues.\n"
"It only plays message types related to Cartographer.\n");
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
ros::init(argc, argv, "rosbag_publisher");
ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
rosbag::Bag bag;
bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
rosbag::View view(bag);
ros::NodeHandle node_handle;
bool use_sim_time;
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 "
"rclcpp::Time and message header times or weird behavior.";
}
std::map<std::string, ros::Publisher> topic_to_publisher;
for (const rosbag::ConnectionInfo* c : view.getConnections()) {
const std::string& topic = c->topic;
if (topic_to_publisher.count(topic) == 0) {
ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
c->datatype, c->msg_def);
topic_to_publisher[topic] = node_handle.advertise(options);
}
}
ros::Duration(1).sleep();
CHECK(ros::ok());
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;
}
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<sensor_msgs::msg::PointCloud2>()) {
PublishWithModifiedTimestamp(
message.instantiate<sensor_msgs::msg::PointCloud2>(), publisher,
bag_to_current);
} else if (message.isType<sensor_msgs::msg::MultiEchoLaserScan>()) {
PublishWithModifiedTimestamp(
message.instantiate<sensor_msgs::msg::MultiEchoLaserScan>(), publisher,
bag_to_current);
} else if (message.isType<sensor_msgs::msg::LaserScan>()) {
PublishWithModifiedTimestamp(
message.instantiate<sensor_msgs::msg::LaserScan>(), publisher,
bag_to_current);
} else if (message.isType<sensor_msgs::msg::Imu>()) {
PublishWithModifiedTimestamp(message.instantiate<sensor_msgs::msg::Imu>(),
publisher, bag_to_current);
} else if (message.isType<nav_msgs::msg::Odometry>()) {
PublishWithModifiedTimestamp(message.instantiate<nav_msgs::msg::Odometry>(),
publisher, bag_to_current);
} else if (message.isType<tf2_msgs::TFMessage>()) {
PublishWithModifiedTimestamp(message.instantiate<tf2_msgs::TFMessage>(),
publisher, bag_to_current);
} else {
LOG(WARNING) << "Skipping message with type " << message.getDataType();
}
rclcpp::Time current_time = rclcpp::Clock().now();
double simulation_delay = cartographer::common::ToSeconds(
cartographer_ros::FromRos(current_time) -
cartographer_ros::FromRos(planned_publish_time));
if (std::abs(simulation_delay) > 0.001) {
LOG(WARNING) << "Playback delayed by " << simulation_delay
<< " s. planned_publish_time: " << planned_publish_time
<< " current_time: " << current_time;
}
}
bag.close();
ros::shutdown();
}
cartographer_ros/cartographer_ros/src/dev/trajectory_comparison_main.cpp
+143
View File
@@ -0,0 +1,143 @@
/*
* 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 <algorithm>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include "cartographer/common/math.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "ros/ros.h"
#include "ros/time.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "tf2_eigen/tf2_eigen.hpp"
#include "tf2_msgs/TFMessage.h"
DEFINE_string(bag_filename, "",
"Bag file containing TF messages of the trajectory that will be "
"compared against the trajectory in the .pbstream file.");
DEFINE_string(tf_parent_frame, "map",
"The parent frame ID of the TF trajectory from the bag file.");
DEFINE_string(tf_child_frame, "base_link",
"The child frame ID of the TF trajectory from the bag file.");
DEFINE_string(pbstream_filename, "",
"Proto stream file containing the pose graph. The last "
"trajectory will be used for comparison.");
namespace cartographer_ros {
namespace {
double FractionSmallerThan(const std::vector<double>& v, double x) {
return static_cast<double>(std::count_if(
v.begin(), v.end(), [=](double value) { return value < x; })) /
v.size();
}
std::string QuantilesToString(std::vector<double>* v) {
if (v->empty()) return "(empty vector)";
std::sort(v->begin(), v->end());
std::stringstream result;
const int kNumQuantiles = 10;
for (int i = 0; i < kNumQuantiles; ++i) {
auto value = v->at(v->size() * i / kNumQuantiles);
auto percentage = 100 * i / kNumQuantiles;
result << percentage << "%: " << value << "\n";
}
result << "100%: " << v->back() << "\n";
return result.str();
}
void Run(const std::string& pbstream_filename,
const std::string& bag_filename) {
cartographer::mapping::proto::PoseGraph pose_graph_proto =
cartographer::io::DeserializePoseGraphFromFile(pbstream_filename);
const cartographer::mapping::proto::Trajectory& last_trajectory_proto =
*pose_graph_proto.mutable_trajectory()->rbegin();
const cartographer::transform::TransformInterpolationBuffer
transform_interpolation_buffer(last_trajectory_proto);
rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read);
rosbag::View view(bag);
std::vector<double> deviation_translation, deviation_rotation;
const double signal_maximum = std::numeric_limits<double>::max();
for (const rosbag::MessageInstance& message : view) {
if (!message.isType<tf2_msgs::TFMessage>()) {
continue;
}
auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
for (const auto& transform : tf_message->transforms) {
if (transform.header.frame_id != FLAGS_tf_parent_frame ||
transform.child_frame_id != FLAGS_tf_child_frame) {
continue;
}
const cartographer::common::Time transform_time =
FromRos(message.getTime());
if (!transform_interpolation_buffer.Has(transform_time)) {
deviation_translation.push_back(signal_maximum);
deviation_rotation.push_back(signal_maximum);
continue;
}
auto optimized_transform =
transform_interpolation_buffer.Lookup(transform_time);
auto published_transform = ToRigid3d(transform);
deviation_translation.push_back((published_transform.translation() -
optimized_transform.translation())
.norm());
deviation_rotation.push_back(
published_transform.rotation().angularDistance(
optimized_transform.rotation()));
}
}
bag.close();
LOG(INFO) << "Distribution of translation difference:\n"
<< QuantilesToString(&deviation_translation);
LOG(INFO) << "Distribution of rotation difference:\n"
<< QuantilesToString(&deviation_rotation);
LOG(INFO) << "Fraction of translation difference smaller than 1m: "
<< FractionSmallerThan(deviation_translation, 1);
LOG(INFO) << "Fraction of translation difference smaller than 0.1m: "
<< FractionSmallerThan(deviation_translation, 0.1);
LOG(INFO) << "Fraction of translation difference smaller than 0.05m: "
<< FractionSmallerThan(deviation_translation, 0.05);
LOG(INFO) << "Fraction of translation difference smaller than 0.01m: "
<< FractionSmallerThan(deviation_translation, 0.01);
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
FLAGS_alsologtostderr = true;
google::InitGoogleLogging(argv[0]);
google::SetUsageMessage(
"\n\n"
"This compares a trajectory from a bag file against the "
"last trajectory in a pbstream file.\n");
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_bag_filename);
}
cartographer_ros/cartographer_ros/src/map_builder_bridge.cpp
+543
View File
@@ -0,0 +1,543 @@
/*
* 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/map_builder_bridge.h"
#include "absl/memory/memory.h"
#include "cartographer/io/color.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros_msgs/msg/status_code.hpp"
#include "cartographer_ros_msgs/msg/status_response.hpp"
namespace cartographer_ros {
namespace {
using ::cartographer::transform::Rigid3d;
constexpr double kTrajectoryLineStripMarkerScale = 0.07;
constexpr double kLandmarkMarkerScale = 0.2;
constexpr double kConstraintMarkerScale = 0.025;
::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];
result.a = 1.f;
return result;
}
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::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;
marker.pose.orientation.w = 1.;
marker.pose.position.z = 0.05;
return marker;
}
int GetLandmarkIndex(
const std::string& landmark_id,
std::unordered_map<std::string, int>* landmark_id_to_index) {
auto it = landmark_id_to_index->find(landmark_id);
if (it == landmark_id_to_index->end()) {
const int new_index = landmark_id_to_index->size();
landmark_id_to_index->emplace(landmark_id, new_index);
return new_index;
}
return it->second;
}
visualization_msgs::msg::Marker CreateLandmarkMarker(int landmark_index,
const Rigid3d& landmark_pose,
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::msg::Marker::SPHERE;
marker.header.stamp = node_time;
marker.header.frame_id = frame_id;
marker.scale.x = kLandmarkMarkerScale;
marker.scale.y = kLandmarkMarkerScale;
marker.scale.z = kLandmarkMarkerScale;
marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
marker.pose = ToGeometryMsgPose(landmark_pose);
return marker;
}
void PushAndResetLineMarker(visualization_msgs::msg::Marker* marker,
std::vector<visualization_msgs::msg::Marker>* markers) {
markers->push_back(*marker);
++marker->id;
marker->points.clear();
}
} // namespace
MapBuilderBridge::MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(std::move(map_builder)),
tf_buffer_(tf_buffer) {}
void MapBuilderBridge::LoadState(const std::string& state_filename,
bool load_frozen_state) {
// Check if suffix of the state file is ".pbstream".
const std::string suffix = ".pbstream";
CHECK_EQ(state_filename.substr(
std::max<int>(state_filename.size() - suffix.size(), 0)),
suffix)
<< "The file containing the state to be loaded must be a "
".pbstream file.";
LOG(INFO) << "Loading saved state '" << state_filename << "'...";
cartographer::io::ProtoStreamReader stream(state_filename);
map_builder_->LoadState(&stream, load_frozen_state);
}
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
[this](const int trajectory_id, const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
// Make sure there is a trajectory with 'trajectory_id'.
CHECK(GetTrajectoryStates().count(trajectory_id));
map_builder_->FinishTrajectory(trajectory_id);
sensor_bridges_.erase(trajectory_id);
}
void MapBuilderBridge::RunFinalOptimization() {
LOG(INFO) << "Running final trajectory optimization...";
map_builder_->pose_graph()->RunFinalOptimization();
}
bool MapBuilderBridge::SerializeState(const std::string& filename,
const bool include_unfinished_submaps) {
return map_builder_->SerializeStateToFile(include_unfinished_submaps,
filename);
}
void MapBuilderBridge::HandleSubmapQuery(
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};
const std::string error =
map_builder_->SubmapToProto(submap_id, &response_proto);
if (!error.empty()) {
LOG(ERROR) << error;
response->status.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND;
response->status.message = error;
return;
}
response->submap_version = response_proto.submap_version();
for (const auto& texture_proto : response_proto.textures()) {
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();
texture.height = texture_proto.height();
texture.resolution = texture_proto.resolution();
texture.slice_pose = ToGeometryMsgPose(
cartographer::transform::ToRigid3(texture_proto.slice_pose()));
}
response->status.message = "Success.";
response->status.code = cartographer_ros_msgs::msg::StatusCode::OK;
}
std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();
// Add active trajectories that are not yet in the pose graph, but are e.g.
// waiting for input sensor data and thus already have a sensor bridge.
for (const auto& sensor_bridge : sensor_bridges_) {
trajectory_states.insert(std::make_pair(
sensor_bridge.first,
::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE));
}
return trajectory_states;
}
cartographer_ros_msgs::msg::SubmapList MapBuilderBridge::GetSubmapList(rclcpp::Time node_time) {
cartographer_ros_msgs::msg::SubmapList submap_list;
submap_list.header.stamp = node_time;
submap_list.header.frame_id = node_options_.map_frame;
for (const auto& submap_id_pose :
map_builder_->pose_graph()->GetAllSubmapPoses()) {
cartographer_ros_msgs::msg::SubmapEntry submap_entry;
submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen(
submap_id_pose.id.trajectory_id);
submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
submap_entry.submap_index = submap_id_pose.id.submap_index;
submap_entry.submap_version = submap_id_pose.data.version;
submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
submap_list.submap.push_back(submap_entry);
}
return submap_list;
}
std::unordered_map<int, MapBuilderBridge::LocalTrajectoryData>
MapBuilderBridge::GetLocalTrajectoryData() {
std::unordered_map<int, LocalTrajectoryData> local_trajectory_data;
for (const auto& entry : sensor_bridges_) {
const int trajectory_id = entry.first;
const SensorBridge& sensor_bridge = *entry.second;
std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data;
{
absl::MutexLock lock(&mutex_);
if (local_slam_data_.count(trajectory_id) == 0) {
continue;
}
local_slam_data = local_slam_data_.at(trajectory_id);
}
// Make sure there is a trajectory with 'trajectory_id'.
CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
local_trajectory_data[trajectory_id] = {
local_slam_data,
map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking(
local_slam_data->time,
trajectory_options_[trajectory_id].published_frame),
trajectory_options_[trajectory_id]};
}
return local_trajectory_data;
}
void MapBuilderBridge::HandleTrajectoryQuery(
const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request,
cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response) {
// This query is safe if the trajectory doesn't exist (returns 0 poses).
// However, we can filter unwanted states at the higher level in the node.
const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
for (const auto& node_id_data :
node_poses.trajectory(request->trajectory_id)) {
if (!node_id_data.data.constant_pose_data.has_value()) {
continue;
}
geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.header.frame_id = node_options_.map_frame;
pose_stamped.header.stamp =
ToRos(node_id_data.data.constant_pose_data.value().time);
pose_stamped.pose = ToGeometryMsgPose(node_id_data.data.global_pose);
response->trajectory.push_back(pose_stamped);
}
response->status.code = cartographer_ros_msgs::msg::StatusCode::OK;
response->status.message =
"Retrieved " + std::to_string(response->trajectory.size()) +
" trajectory nodes from trajectory " + std::to_string(request->trajectory_id) + ".";
}
visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList(
rclcpp::Time node_time)
{
visualization_msgs::msg::MarkerArray 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.
std::map<int, int /* node_index */>
trajectory_to_last_inter_submap_constrained_node;
std::map<int, int /* node_index */>
trajectory_to_last_inter_trajectory_constrained_node;
for (const int trajectory_id : node_poses.trajectory_ids()) {
trajectory_to_last_inter_submap_constrained_node[trajectory_id] = 0;
trajectory_to_last_inter_trajectory_constrained_node[trajectory_id] = 0;
}
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints) {
if (constraint.tag ==
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
.trajectory_id] =
std::max(trajectory_to_last_inter_submap_constrained_node.at(
constraint.node_id.trajectory_id),
constraint.node_id.node_index);
} else {
trajectory_to_last_inter_trajectory_constrained_node
[constraint.node_id.trajectory_id] =
std::max(trajectory_to_last_inter_submap_constrained_node.at(
constraint.node_id.trajectory_id),
constraint.node_id.node_index);
}
}
}
for (const int trajectory_id : node_poses.trajectory_ids()) {
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));
int last_inter_trajectory_constrained_node = std::max(
node_poses.trajectory(trajectory_id).begin()->id.node_index,
trajectory_to_last_inter_trajectory_constrained_node.at(trajectory_id));
last_inter_submap_constrained_node =
std::max(last_inter_submap_constrained_node,
last_inter_trajectory_constrained_node);
if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
last_inter_submap_constrained_node =
(--node_poses.trajectory(trajectory_id).end())->id.node_index;
last_inter_trajectory_constrained_node =
last_inter_submap_constrained_node;
}
marker.color.a = 1.0;
for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
if (!node_id_data.data.constant_pose_data.has_value()) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
continue;
}
const ::geometry_msgs::msg::Point node_point =
ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
marker.points.push_back(node_point);
if (node_id_data.id.node_index ==
last_inter_trajectory_constrained_node) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
marker.points.push_back(node_point);
marker.color.a = 0.5;
}
if (node_id_data.id.node_index == last_inter_submap_constrained_node) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
marker.points.push_back(node_point);
marker.color.a = 0.25;
}
// Work around the 16384 point limit in RViz by splitting the
// trajectory into multiple markers.
if (marker.points.size() == 16384) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
// Push back the last point, so the two markers appear connected.
marker.points.push_back(node_point);
}
}
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
size_t current_last_marker_id = static_cast<size_t>(marker.id - 1);
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::msg::Marker::DELETE;
while (static_cast<size_t>(marker.id) <=
trajectory_to_highest_marker_id_[trajectory_id]) {
trajectory_node_list.markers.push_back(marker);
++marker.id;
}
trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
}
}
return trajectory_node_list;
}
visualization_msgs::msg::MarkerArray MapBuilderBridge::GetLandmarkPosesList(
rclcpp::Time node_time)
{
visualization_msgs::msg::MarkerArray landmark_poses_list;
const std::map<std::string, Rigid3d> 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, node_time));
}
return landmark_poses_list;
}
visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(rclcpp::Time node_time) {
visualization_msgs::msg::MarkerArray constraint_list;
int marker_id = 0;
visualization_msgs::msg::Marker constraint_intra_marker;
constraint_intra_marker.id = marker_id++;
constraint_intra_marker.ns = "Intra constraints";
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::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
// above the intra constraints marker in order to ensure that they are
// visible.
residual_intra_marker.pose.position.z = 0.1;
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::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::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::msg::Marker residual_inter_diff_trajectory_marker =
constraint_intra_marker;
residual_inter_diff_trajectory_marker.id = marker_id++;
residual_inter_diff_trajectory_marker.ns =
"Inter residuals, different trajectories";
residual_inter_diff_trajectory_marker.pose.position.z = 0.1;
const auto trajectory_node_poses =
map_builder_->pose_graph()->GetTrajectoryNodePoses();
const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints) {
visualization_msgs::msg::Marker *constraint_marker, *residual_marker;
std_msgs::msg::ColorRGBA color_constraint, color_residual;
if (constraint.tag ==
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
// to ensure different starting colors. Also add a fixed offset of 25
// to avoid having identical colors as trajectories.
color_constraint = ToMessage(
cartographer::io::GetColor(constraint.submap_id.submap_index +
constraint.submap_id.trajectory_id + 25));
color_residual.a = 1.0;
color_residual.r = 1.0;
} else {
if (constraint.node_id.trajectory_id ==
constraint.submap_id.trajectory_id) {
constraint_marker = &constraint_inter_same_trajectory_marker;
residual_marker = &residual_inter_same_trajectory_marker;
// Bright yellow
color_constraint.a = 1.0;
color_constraint.r = color_constraint.g = 1.0;
} else {
constraint_marker = &constraint_inter_diff_trajectory_marker;
residual_marker = &residual_inter_diff_trajectory_marker;
// Bright orange
color_constraint.a = 1.0;
color_constraint.r = 1.0;
color_constraint.g = 165. / 255.;
}
// Bright cyan
color_residual.a = 1.0;
color_residual.b = color_residual.g = 1.0;
}
for (int i = 0; i < 2; ++i) {
constraint_marker->colors.push_back(color_constraint);
residual_marker->colors.push_back(color_residual);
}
const auto submap_it = submap_poses.find(constraint.submap_id);
if (submap_it == submap_poses.end()) {
continue;
}
const auto& submap_pose = submap_it->data.pose;
const auto node_it = trajectory_node_poses.find(constraint.node_id);
if (node_it == trajectory_node_poses.end()) {
continue;
}
const auto& trajectory_node_pose = node_it->data.global_pose;
const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;
constraint_marker->points.push_back(
ToGeometryMsgPoint(submap_pose.translation()));
constraint_marker->points.push_back(
ToGeometryMsgPoint(constraint_pose.translation()));
residual_marker->points.push_back(
ToGeometryMsgPoint(constraint_pose.translation()));
residual_marker->points.push_back(
ToGeometryMsgPoint(trajectory_node_pose.translation()));
}
constraint_list.markers.push_back(constraint_intra_marker);
constraint_list.markers.push_back(residual_intra_marker);
constraint_list.markers.push_back(constraint_inter_same_trajectory_marker);
constraint_list.markers.push_back(residual_inter_same_trajectory_marker);
constraint_list.markers.push_back(constraint_inter_diff_trajectory_marker);
constraint_list.markers.push_back(residual_inter_diff_trajectory_marker);
return constraint_list;
}
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
void MapBuilderBridge::OnLocalSlamResult(
const int trajectory_id, const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local) {
std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
std::make_shared<LocalTrajectoryData::LocalSlamData>(
LocalTrajectoryData::LocalSlamData{time, local_pose,
std::move(range_data_in_local)});
absl::MutexLock lock(&mutex_);
local_slam_data_[trajectory_id] = std::move(local_slam_data);
}
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/metrics/family_factory.cpp
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/metrics/family_factory.h"
#include "absl/memory/memory.h"
namespace cartographer_ros {
namespace metrics {
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
::cartographer::metrics::Family<::cartographer::metrics::Counter>*
FamilyFactory::NewCounterFamily(const std::string& name,
const std::string& description) {
auto wrapper = absl::make_unique<CounterFamily>(name, description);
auto* ptr = wrapper.get();
counter_families_.emplace_back(std::move(wrapper));
return ptr;
}
::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
FamilyFactory::NewGaugeFamily(const std::string& name,
const std::string& description) {
auto wrapper = absl::make_unique<GaugeFamily>(name, description);
auto* ptr = wrapper.get();
gauge_families_.emplace_back(std::move(wrapper));
return ptr;
}
::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
FamilyFactory::NewHistogramFamily(const std::string& name,
const std::string& description,
const BucketBoundaries& boundaries) {
auto wrapper =
absl::make_unique<HistogramFamily>(name, description, boundaries);
auto* ptr = wrapper.get();
histogram_families_.emplace_back(std::move(wrapper));
return ptr;
}
void FamilyFactory::ReadMetrics(
cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const {
for (const auto& counter_family : counter_families_) {
response->metric_families.push_back(counter_family->ToRosMessage());
}
for (const auto& gauge_family : gauge_families_) {
response->metric_families.push_back(gauge_family->ToRosMessage());
}
for (const auto& histogram_family : histogram_families_) {
response->metric_families.push_back(histogram_family->ToRosMessage());
}
}
} // namespace metrics
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/metrics/internal/family.cpp
+82
View File
@@ -0,0 +1,82 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/metrics/internal/family.h"
#include "absl/memory/memory.h"
#include "cartographer_ros/metrics/internal/counter.h"
#include "cartographer_ros/metrics/internal/gauge.h"
#include "cartographer_ros/metrics/internal/histogram.h"
namespace cartographer_ros {
namespace metrics {
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
Counter* CounterFamily::Add(const std::map<std::string, std::string>& labels) {
auto wrapper = absl::make_unique<Counter>(labels);
auto* ptr = wrapper.get();
wrappers_.emplace_back(std::move(wrapper));
return ptr;
}
cartographer_ros_msgs::msg::MetricFamily CounterFamily::ToRosMessage() {
cartographer_ros_msgs::msg::MetricFamily family_msg;
family_msg.name = name_;
family_msg.description = description_;
for (const auto& wrapper : wrappers_) {
family_msg.metrics.push_back(wrapper->ToRosMessage());
}
return family_msg;
}
Gauge* GaugeFamily::Add(const std::map<std::string, std::string>& labels) {
auto wrapper = absl::make_unique<Gauge>(labels);
auto* ptr = wrapper.get();
wrappers_.emplace_back(std::move(wrapper));
return ptr;
}
cartographer_ros_msgs::msg::MetricFamily GaugeFamily::ToRosMessage() {
cartographer_ros_msgs::msg::MetricFamily family_msg;
family_msg.name = name_;
family_msg.description = description_;
for (const auto& wrapper : wrappers_) {
family_msg.metrics.push_back(wrapper->ToRosMessage());
}
return family_msg;
}
Histogram* HistogramFamily::Add(
const std::map<std::string, std::string>& labels) {
auto wrapper = absl::make_unique<Histogram>(labels, boundaries_);
auto* ptr = wrapper.get();
wrappers_.emplace_back(std::move(wrapper));
return ptr;
}
cartographer_ros_msgs::msg::MetricFamily HistogramFamily::ToRosMessage() {
cartographer_ros_msgs::msg::MetricFamily family_msg;
family_msg.name = name_;
family_msg.description = description_;
for (const auto& wrapper : wrappers_) {
family_msg.metrics.push_back(wrapper->ToRosMessage());
}
return family_msg;
}
} // namespace metrics
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/metrics/internal/histogram.cpp
+90
View File
@@ -0,0 +1,90 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/metrics/internal/histogram.h"
#include <algorithm>
#include <numeric>
#include "glog/logging.h"
namespace cartographer_ros {
namespace metrics {
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
Histogram::Histogram(const std::map<std::string, std::string>& labels,
const BucketBoundaries& bucket_boundaries)
: labels_(labels),
bucket_boundaries_(bucket_boundaries),
bucket_counts_(bucket_boundaries.size() + 1) {
absl::MutexLock lock(&mutex_);
CHECK(std::is_sorted(std::begin(bucket_boundaries_),
std::end(bucket_boundaries_)));
}
void Histogram::Observe(double value) {
auto bucket_index =
std::distance(bucket_boundaries_.begin(),
std::upper_bound(bucket_boundaries_.begin(),
bucket_boundaries_.end(), value));
absl::MutexLock lock(&mutex_);
sum_ += value;
bucket_counts_[bucket_index] += 1;
}
std::map<double, double> Histogram::CountsByBucket() {
absl::MutexLock lock(&mutex_);
std::map<double, double> counts_by_bucket;
// Add the finite buckets.
for (size_t i = 0; i < bucket_boundaries_.size(); ++i) {
counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i);
}
// Add the "infinite" bucket.
counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back();
return counts_by_bucket;
}
double Histogram::Sum() {
absl::MutexLock lock(&mutex_);
return sum_;
}
double Histogram::CumulativeCount() {
absl::MutexLock lock(&mutex_);
return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.);
}
cartographer_ros_msgs::msg::Metric Histogram::ToRosMessage() {
cartographer_ros_msgs::msg::Metric msg;
msg.type = cartographer_ros_msgs::msg::Metric::TYPE_HISTOGRAM;
for (const auto& label : labels_) {
cartographer_ros_msgs::msg::MetricLabel label_msg;
label_msg.key = label.first;
label_msg.value = label.second;
msg.labels.push_back(label_msg);
}
for (const auto& bucket : CountsByBucket()) {
cartographer_ros_msgs::msg::HistogramBucket bucket_msg;
bucket_msg.bucket_boundary = bucket.first;
bucket_msg.count = bucket.second;
msg.counts_by_bucket.push_back(bucket_msg);
}
return msg;
}
} // namespace metrics
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/metrics/internal/metrics_test.cpp
+104
View File
@@ -0,0 +1,104 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <algorithm>
#include <array>
#include <numeric>
#include "cartographer/metrics/histogram.h"
#include "cartographer_ros/metrics/internal/counter.h"
#include "cartographer_ros/metrics/internal/gauge.h"
#include "cartographer_ros/metrics/internal/histogram.h"
#include "gtest/gtest.h"
namespace cartographer_ros {
namespace metrics {
TEST(Metrics, GaugeTest) {
Gauge gauge({});
EXPECT_EQ(gauge.Value(), 0.);
gauge.Increment(1.2);
EXPECT_EQ(gauge.Value(), 1.2);
gauge.Increment();
EXPECT_EQ(gauge.Value(), 2.2);
gauge.Decrement(2.2);
EXPECT_EQ(gauge.Value(), 0.);
gauge.Decrement();
EXPECT_EQ(gauge.Value(), -1.);
}
TEST(Metrics, CounterTest) {
Counter counter({});
EXPECT_EQ(counter.Value(), 0.);
counter.Increment(1.2);
EXPECT_EQ(counter.Value(), 1.2);
counter.Increment(0.8);
EXPECT_EQ(counter.Value(), 2.);
counter.Increment();
EXPECT_EQ(counter.Value(), 3.);
}
TEST(Metrics, HistogramFixedWidthTest) {
auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3);
Histogram histogram({}, boundaries);
// Observe some values that fit in finite buckets.
std::array<double, 3> values = {{0., 2, 2.5}};
for (const auto& value : values) {
histogram.Observe(value);
}
// 1 2 3 inf
// 1 | 0 | 2 | 0 |
EXPECT_EQ(histogram.CountsByBucket()[1], 1);
EXPECT_EQ(histogram.CountsByBucket()[2], 0);
EXPECT_EQ(histogram.CountsByBucket()[3], 2);
EXPECT_EQ(histogram.CumulativeCount(), values.size());
EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
// Values above the last bucket boundary should go to the "infinite" bucket.
histogram.Observe(3.5);
// 1 2 3 inf
// 1 | 0 | 2 | 1 |
EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
}
TEST(Metrics, HistogramScaledPowersOfTest) {
auto boundaries =
::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048);
Histogram histogram({}, boundaries);
// Observe some values that fit in finite buckets.
std::array<double, 3> values = {{256, 512, 666}};
for (const auto& value : values) {
histogram.Observe(value);
}
// ... 256 512 1024 inf
// ... | 1 | 2 | 0 |
EXPECT_EQ(histogram.CountsByBucket()[256], 0);
EXPECT_EQ(histogram.CountsByBucket()[512], 1);
EXPECT_EQ(histogram.CountsByBucket()[1024], 2);
EXPECT_EQ(histogram.CumulativeCount(), values.size());
EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
// Values above the last bucket boundary should go to the "infinite" bucket.
histogram.Observe(2048);
// ... 256 512 1024 inf
// ... | 1 | 2 | 1 |
EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
}
} // namespace metrics
} // namespace cartographer_ros
cartographer_ros/cartographer_ros/src/msg_conversion.cpp
+421
View File
@@ -0,0 +1,421 @@
/*
* 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/msg_conversion.h"
#include <cmath>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/transform/proto/transform.pb.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/time_conversion.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/msg/occupancy_grid.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "builtin_interfaces/msg/time.hpp"
//#include "ros/serialization.h"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/multi_echo_laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
namespace {
// Sizes of PCL point types have to be 4n floats for alignment, as described in
// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php
struct PointXYZT {
float x;
float y;
float z;
float time;
};
struct PointXYZIT {
PCL_ADD_POINT4D;
float intensity;
float time;
float unused_padding[2];
};
} // namespace
POINT_CLOUD_REGISTER_POINT_STRUCT(
PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time))
POINT_CLOUD_REGISTER_POINT_STRUCT(
PointXYZIT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity,
intensity)(float, time, time))
namespace cartographer_ros {
namespace {
// The ros::sensor_msgs::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.;
using ::cartographer::sensor::LandmarkData;
using ::cartographer::sensor::LandmarkObservation;
using ::cartographer::sensor::PointCloudWithIntensities;
using ::cartographer::transform::Rigid3d;
using ::cartographer_ros_msgs::msg::LandmarkEntry;
using ::cartographer_ros_msgs::msg::LandmarkList;
sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
const std::string& frame_id,
const int num_points) {
sensor_msgs::msg::PointCloud2 msg;
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
msg.header.frame_id = frame_id;
msg.height = 1;
msg.width = num_points;
msg.fields.resize(3);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
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::msg::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.is_bigendian = false;
msg.point_step = 16;
msg.row_step = 16 * msg.width;
msg.is_dense = true;
msg.data.resize(16 * num_points);
return msg;
}
// For sensor_msgs::msg::LaserScan.
bool HasEcho(float) { return true; }
float GetFirstEcho(float range) { return range; }
// For sensor_msgs::msg::MultiEchoLaserScan.
bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) {
return !echo.echoes.empty();
}
float GetFirstEcho(const sensor_msgs::msg::LaserEcho& echo) {
return echo.echoes[0];
}
// For sensor_msgs::msg::LaserScan and sensor_msgs::msg::MultiEchoLaserScan.
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
CHECK_GE(msg.range_min, 0.f);
CHECK_GE(msg.range_max, msg.range_min);
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
const cartographer::sensor::TimedRangefinderPoint point{
rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment};
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
angle += msg.angle_increment;
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration);
for (auto& point : point_cloud.points) {
point.time -= duration;
}
}
return std::make_tuple(point_cloud, timestamp);
}
bool PointCloud2HasField(const sensor_msgs::msg::PointCloud2& pc2,
const std::string& field_name) {
for (const auto& field : pc2.fields) {
if (field.name == field_name) {
return true;
}
}
return false;
}
} // namespace
sensor_msgs::msg::PointCloud2 ToPointCloud2Message(
const int64_t timestamp, const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& point_cloud) {
auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
size_t offset = 0;
float * const data = reinterpret_cast<float*>(&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::msg::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg) {
PointCloudWithIntensities point_cloud;
// We check for intensity field here to avoid run-time warnings if we pass in
// a PointCloud2 without intensity.
if (PointCloud2HasField(msg, "intensity")) {
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZIT> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(point.intensity);
}
} else {
pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(point.intensity);
}
}
} else {
// If we don't have an intensity field, just copy XYZ and fill in 1.0f.
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZT> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(1.0f);
}
} else {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(1.0f);
}
}
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration);
for (auto& point : point_cloud.points) {
point.time -= duration;
CHECK_LE(point.time, 0.f)
<< "Encountered a point with a larger stamp than "
"the last point in the cloud.";
}
}
return std::make_tuple(point_cloud, timestamp);
}
LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
LandmarkData landmark_data;
landmark_data.time = FromRos(landmark_list.header.stamp);
for (const LandmarkEntry& entry : landmark_list.landmarks) {
landmark_data.landmark_observations.push_back(
{entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
entry.translation_weight, entry.rotation_weight});
}
return landmark_data;
}
Rigid3d ToRigid3d(const geometry_msgs::msg::TransformStamped& transform) {
return Rigid3d(ToEigen(transform.transform.translation),
ToEigen(transform.transform.rotation));
}
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::msg::Vector3& vector3) {
return Eigen::Vector3d(vector3.x, vector3.y, vector3.z);
}
Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) {
return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
quaternion.z);
}
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();
transform.rotation.w = rigid3d.rotation().w();
transform.rotation.x = rigid3d.rotation().x();
transform.rotation.y = rigid3d.rotation().y();
transform.rotation.z = rigid3d.rotation().z();
return transform;
}
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();
pose.orientation.y = rigid3d.rotation().y();
pose.orientation.z = rigid3d.rotation().z();
return pose;
}
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();
return point;
}
Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude,
const double altitude) {
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
constexpr double a = 6378137.; // semi-major axis, equator to center.
constexpr double f = 1. / 298.257223563;
constexpr double b = a * (1. - f); // semi-minor axis, pole to center.
constexpr double a_squared = a * a;
constexpr double b_squared = b * b;
constexpr double e_squared = (a_squared - b_squared) / a_squared;
const double sin_phi = std::sin(cartographer::common::DegToRad(latitude));
const double cos_phi = std::cos(cartographer::common::DegToRad(latitude));
const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude));
const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude));
const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi);
const double x = (N + altitude) * cos_phi * cos_lambda;
const double y = (N + altitude) * cos_phi * sin_lambda;
const double z = (b_squared / a_squared * N + altitude) * sin_phi;
return Eigen::Vector3d(x, y, z);
}
cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(
const double latitude, const double longitude) {
const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.);
const Eigen::Quaterniond rotation =
Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.),
Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude),
Eigen::Vector3d::UnitZ());
return cartographer::transform::Rigid3d(rotation * -translation, rotation);
}
std::unique_ptr<nav_msgs::msg::OccupancyGrid> CreateOccupancyGridMsg(
const cartographer::io::PaintSubmapSlicesResult& painted_slices,
const double resolution, const std::string& frame_id,
const rclcpp::Time& time) {
auto occupancy_grid = absl::make_unique<nav_msgs::msg::OccupancyGrid>();
const int width = cairo_image_surface_get_width(painted_slices.surface.get());
const int height =
cairo_image_surface_get_height(painted_slices.surface.get());
occupancy_grid->header.stamp = time;
occupancy_grid->header.frame_id = frame_id;
occupancy_grid->info.map_load_time = time;
occupancy_grid->info.resolution = resolution;
occupancy_grid->info.width = width;
occupancy_grid->info.height = height;
occupancy_grid->info.origin.position.x =
-painted_slices.origin.x() * resolution;
occupancy_grid->info.origin.position.y =
(-height + painted_slices.origin.y()) * resolution;
occupancy_grid->info.origin.position.z = 0.;
occupancy_grid->info.origin.orientation.w = 1.;
occupancy_grid->info.origin.orientation.x = 0.;
occupancy_grid->info.origin.orientation.y = 0.;
occupancy_grid->info.origin.orientation.z = 0.;
const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
cairo_image_surface_get_data(painted_slices.surface.get()));
occupancy_grid->data.reserve(width * height);
for (int y = height - 1; y >= 0; --y) {
for (int x = 0; x < width; ++x) {
const uint32_t packed = pixel_data[y * width + x];
const unsigned char color = packed >> 16;
const unsigned char observed = packed >> 8;
const int value =
observed == 0
? -1
: ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
CHECK_LE(-1, value);
CHECK_GE(100, value);
occupancy_grid->data.push_back(value);
}
}
return occupancy_grid;
}
} // namespace cartographer_ros

Some files were not shown because too many files have changed in this diff Show More