HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Localizations/Packages/slam_toolbox/Dockerfile Normal file
View File

@@ -0,0 +1,26 @@
FROM ros:melodic-ros-base-bionic
# USE BASH
SHELL ["/bin/bash", "-c"]
# RUN LINE BELOW TO REMOVE debconf ERRORS (MUST RUN BEFORE ANY apt-get CALLS)
RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends apt-utils
# slam_toolbox
RUN mkdir -p catkin_ws/src
RUN cd catkin_ws/src && git clone -b noetic-devel https://github.com/SteveMacenski/slam_toolbox.git
RUN source /opt/ros/melodic/setup.bash \
&& cd catkin_ws \
&& rosdep update \
&& rosdep install -y -r --from-paths src --ignore-src --rosdistro=melodic -y
RUN apt install python-catkin-tools -y
RUN source /opt/ros/melodic/setup.bash \
&& cd catkin_ws/src \
&& catkin_init_workspace \
&& cd .. \
&& catkin config --install \
&& catkin build -DCMAKE_BUILD_TYPE=Release

504
Localizations/Packages/slam_toolbox/LICENSE Normal file
View File

@@ -0,0 +1,504 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 2.1, February 1999
Copyright (C) 1991, 1999 Free Software Foundation, Inc.
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
[This is the first released version of the Lesser GPL. It also counts
as the successor of the GNU Library Public License, version 2, hence
the version number 2.1.]
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
Licenses are intended to guarantee your freedom to share and change
free software--to make sure the software is free for all its users.
This license, the Lesser General Public License, applies to some
specially designated software packages--typically libraries--of the
Free Software Foundation and other authors who decide to use it. You
can use it too, but we suggest you first think carefully about whether
this license or the ordinary General Public License is the better
strategy to use in any particular case, based on the explanations below.
When we speak of free software, we are referring to freedom of use,
not price. Our General Public Licenses are designed to make sure that
you have the freedom to distribute copies of free software (and charge
for this service if you wish); that you receive source code or can get
it if you want it; that you can change the software and use pieces of
it in new free programs; and that you are informed that you can do
these things.
To protect your rights, we need to make restrictions that forbid
distributors to deny you these rights or to ask you to surrender these
rights. These restrictions translate to certain responsibilities for
you if you distribute copies of the library or if you modify it.
For example, if you distribute copies of the library, whether gratis
or for a fee, you must give the recipients all the rights that we gave
you. You must make sure that they, too, receive or can get the source
code. If you link other code with the library, you must provide
complete object files to the recipients, so that they can relink them
with the library after making changes to the library and recompiling
it. And you must show them these terms so they know their rights.
We protect your rights with a two-step method: (1) we copyright the
library, and (2) we offer you this license, which gives you legal
permission to copy, distribute and/or modify the library.
To protect each distributor, we want to make it very clear that
there is no warranty for the free library. Also, if the library is
modified by someone else and passed on, the recipients should know
that what they have is not the original version, so that the original
author's reputation will not be affected by problems that might be
introduced by others.
Finally, software patents pose a constant threat to the existence of
any free program. We wish to make sure that a company cannot
effectively restrict the users of a free program by obtaining a
restrictive license from a patent holder. Therefore, we insist that
any patent license obtained for a version of the library must be
consistent with the full freedom of use specified in this license.
Most GNU software, including some libraries, is covered by the
ordinary GNU General Public License. This license, the GNU Lesser
General Public License, applies to certain designated libraries, and
is quite different from the ordinary General Public License. We use
this license for certain libraries in order to permit linking those
libraries into non-free programs.
When a program is linked with a library, whether statically or using
a shared library, the combination of the two is legally speaking a
combined work, a derivative of the original library. The ordinary
General Public License therefore permits such linking only if the
entire combination fits its criteria of freedom. The Lesser General
Public License permits more lax criteria for linking other code with
the library.
We call this license the "Lesser" General Public License because it
does Less to protect the user's freedom than the ordinary General
Public License. It also provides other free software developers Less
of an advantage over competing non-free programs. These disadvantages
are the reason we use the ordinary General Public License for many
libraries. However, the Lesser license provides advantages in certain
special circumstances.
For example, on rare occasions, there may be a special need to
encourage the widest possible use of a certain library, so that it becomes
a de-facto standard. To achieve this, non-free programs must be
allowed to use the library. A more frequent case is that a free
library does the same job as widely used non-free libraries. In this
case, there is little to gain by limiting the free library to free
software only, so we use the Lesser General Public License.
In other cases, permission to use a particular library in non-free
programs enables a greater number of people to use a large body of
free software. For example, permission to use the GNU C Library in
non-free programs enables many more people to use the whole GNU
operating system, as well as its variant, the GNU/Linux operating
system.
Although the Lesser General Public License is Less protective of the
users' freedom, it does ensure that the user of a program that is
linked with the Library has the freedom and the wherewithal to run
that program using a modified version of the Library.
The precise terms and conditions for copying, distribution and
modification follow. Pay close attention to the difference between a
"work based on the library" and a "work that uses the library". The
former contains code derived from the library, whereas the latter must
be combined with the library in order to run.
GNU LESSER GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License Agreement applies to any software library or other
program which contains a notice placed by the copyright holder or
other authorized party saying it may be distributed under the terms of
this Lesser General Public License (also called "this License").
Each licensee is addressed as "you".
A "library" means a collection of software functions and/or data
prepared so as to be conveniently linked with application programs
(which use some of those functions and data) to form executables.
The "Library", below, refers to any such software library or work
which has been distributed under these terms. A "work based on the
Library" means either the Library or any derivative work under
copyright law: that is to say, a work containing the Library or a
portion of it, either verbatim or with modifications and/or translated
straightforwardly into another language. (Hereinafter, translation is
included without limitation in the term "modification".)
"Source code" for a work means the preferred form of the work for
making modifications to it. For a library, complete source code means
all the source code for all modules it contains, plus any associated
interface definition files, plus the scripts used to control compilation
and installation of the library.
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running a program using the Library is not restricted, and output from
such a program is covered only if its contents constitute a work based
on the Library (independent of the use of the Library in a tool for
writing it). Whether that is true depends on what the Library does
and what the program that uses the Library does.
1. You may copy and distribute verbatim copies of the Library's
complete source code as you receive it, in any medium, provided that
you conspicuously and appropriately publish on each copy an
appropriate copyright notice and disclaimer of warranty; keep intact
all the notices that refer to this License and to the absence of any
warranty; and distribute a copy of this License along with the
Library.
You may charge a fee for the physical act of transferring a copy,
and you may at your option offer warranty protection in exchange for a
fee.
2. You may modify your copy or copies of the Library or any portion
of it, thus forming a work based on the Library, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) The modified work must itself be a software library.
b) You must cause the files modified to carry prominent notices
stating that you changed the files and the date of any change.
c) You must cause the whole of the work to be licensed at no
charge to all third parties under the terms of this License.
d) If a facility in the modified Library refers to a function or a
table of data to be supplied by an application program that uses
the facility, other than as an argument passed when the facility
is invoked, then you must make a good faith effort to ensure that,
in the event an application does not supply such function or
table, the facility still operates, and performs whatever part of
its purpose remains meaningful.
(For example, a function in a library to compute square roots has
a purpose that is entirely well-defined independent of the
application. Therefore, Subsection 2d requires that any
application-supplied function or table used by this function must
be optional: if the application does not supply it, the square
root function must still compute square roots.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Library,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Library, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote
it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Library.
In addition, mere aggregation of another work not based on the Library
with the Library (or with a work based on the Library) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may opt to apply the terms of the ordinary GNU General Public
License instead of this License to a given copy of the Library. To do
this, you must alter all the notices that refer to this License, so
that they refer to the ordinary GNU General Public License, version 2,
instead of to this License. (If a newer version than version 2 of the
ordinary GNU General Public License has appeared, then you can specify
that version instead if you wish.) Do not make any other change in
these notices.
Once this change is made in a given copy, it is irreversible for
that copy, so the ordinary GNU General Public License applies to all
subsequent copies and derivative works made from that copy.
This option is useful when you wish to copy part of the code of
the Library into a program that is not a library.
4. You may copy and distribute the Library (or a portion or
derivative of it, under Section 2) in object code or executable form
under the terms of Sections 1 and 2 above provided that you accompany
it with the complete corresponding machine-readable source code, which
must be distributed under the terms of Sections 1 and 2 above on a
medium customarily used for software interchange.
If distribution of object code is made by offering access to copy
from a designated place, then offering equivalent access to copy the
source code from the same place satisfies the requirement to
distribute the source code, even though third parties are not
compelled to copy the source along with the object code.
5. A program that contains no derivative of any portion of the
Library, but is designed to work with the Library by being compiled or
linked with it, is called a "work that uses the Library". Such a
work, in isolation, is not a derivative work of the Library, and
therefore falls outside the scope of this License.
However, linking a "work that uses the Library" with the Library
creates an executable that is a derivative of the Library (because it
contains portions of the Library), rather than a "work that uses the
library". The executable is therefore covered by this License.
Section 6 states terms for distribution of such executables.
When a "work that uses the Library" uses material from a header file
that is part of the Library, the object code for the work may be a
derivative work of the Library even though the source code is not.
Whether this is true is especially significant if the work can be
linked without the Library, or if the work is itself a library. The
threshold for this to be true is not precisely defined by law.
If such an object file uses only numerical parameters, data
structure layouts and accessors, and small macros and small inline
functions (ten lines or less in length), then the use of the object
file is unrestricted, regardless of whether it is legally a derivative
work. (Executables containing this object code plus portions of the
Library will still fall under Section 6.)
Otherwise, if the work is a derivative of the Library, you may
distribute the object code for the work under the terms of Section 6.
Any executables containing that work also fall under Section 6,
whether or not they are linked directly with the Library itself.
6. As an exception to the Sections above, you may also combine or
link a "work that uses the Library" with the Library to produce a
work containing portions of the Library, and distribute that work
under terms of your choice, provided that the terms permit
modification of the work for the customer's own use and reverse
engineering for debugging such modifications.
You must give prominent notice with each copy of the work that the
Library is used in it and that the Library and its use are covered by
this License. You must supply a copy of this License. If the work
during execution displays copyright notices, you must include the
copyright notice for the Library among them, as well as a reference
directing the user to the copy of this License. Also, you must do one
of these things:
a) Accompany the work with the complete corresponding
machine-readable source code for the Library including whatever
changes were used in the work (which must be distributed under
Sections 1 and 2 above); and, if the work is an executable linked
with the Library, with the complete machine-readable "work that
uses the Library", as object code and/or source code, so that the
user can modify the Library and then relink to produce a modified
executable containing the modified Library. (It is understood
that the user who changes the contents of definitions files in the
Library will not necessarily be able to recompile the application
to use the modified definitions.)
b) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (1) uses at run time a
copy of the library already present on the user's computer system,
rather than copying library functions into the executable, and (2)
will operate properly with a modified version of the library, if
the user installs one, as long as the modified version is
interface-compatible with the version that the work was made with.
c) Accompany the work with a written offer, valid for at
least three years, to give the same user the materials
specified in Subsection 6a, above, for a charge no more
than the cost of performing this distribution.
d) If distribution of the work is made by offering access to copy
from a designated place, offer equivalent access to copy the above
specified materials from the same place.
e) Verify that the user has already received a copy of these
materials or that you have already sent this user a copy.
For an executable, the required form of the "work that uses the
Library" must include any data and utility programs needed for
reproducing the executable from it. However, as a special exception,
the materials to be distributed need not include anything that is
normally distributed (in either source or binary form) with the major
components (compiler, kernel, and so on) of the operating system on
which the executable runs, unless that component itself accompanies
the executable.
It may happen that this requirement contradicts the license
restrictions of other proprietary libraries that do not normally
accompany the operating system. Such a contradiction means you cannot
use both them and the Library together in an executable that you
distribute.
7. You may place library facilities that are a work based on the
Library side-by-side in a single library together with other library
facilities not covered by this License, and distribute such a combined
library, provided that the separate distribution of the work based on
the Library and of the other library facilities is otherwise
permitted, and provided that you do these two things:
a) Accompany the combined library with a copy of the same work
based on the Library, uncombined with any other library
facilities. This must be distributed under the terms of the
Sections above.
b) Give prominent notice with the combined library of the fact
that part of it is a work based on the Library, and explaining
where to find the accompanying uncombined form of the same work.
8. You may not copy, modify, sublicense, link with, or distribute
the Library except as expressly provided under this License. Any
attempt otherwise to copy, modify, sublicense, link with, or
distribute the Library is void, and will automatically terminate your
rights under this License. However, parties who have received copies,
or rights, from you under this License will not have their licenses
terminated so long as such parties remain in full compliance.
9. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Library or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Library (or any work based on the
Library), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Library or works based on it.
10. Each time you redistribute the Library (or any work based on the
Library), the recipient automatically receives a license from the
original licensor to copy, distribute, link with or modify the Library
subject to these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties with
this License.
11. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Library at all. For example, if a patent
license would not permit royalty-free redistribution of the Library by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Library.
If any portion of this section is held invalid or unenforceable under any
particular circumstance, the balance of the section is intended to apply,
and the section as a whole is intended to apply in other circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
12. If the distribution and/or use of the Library is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Library under this License may add
an explicit geographical distribution limitation excluding those countries,
so that distribution is permitted only in or among countries not thus
excluded. In such case, this License incorporates the limitation as if
written in the body of this License.
13. The Free Software Foundation may publish revised and/or new
versions of the Lesser General Public License from time to time.
Such new versions will be similar in spirit to the present version,
but may differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the Library
specifies a version number of this License which applies to it and
"any later version", you have the option of following the terms and
conditions either of that version or of any later version published by
the Free Software Foundation. If the Library does not specify a
license version number, you may choose any version ever published by
the Free Software Foundation.
14. If you wish to incorporate parts of the Library into other free
programs whose distribution conditions are incompatible with these,
write to the author to ask for permission. For software which is
copyrighted by the Free Software Foundation, write to the Free
Software Foundation; we sometimes make exceptions for this. Our
decision will be guided by the two goals of preserving the free status
of all derivatives of our free software and of promoting the sharing
and reuse of software generally.
NO WARRANTY
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Libraries
If you develop a new library, and you want it to be of the greatest
possible use to the public, we recommend making it free software that
everyone can redistribute and change. You can do so by permitting
redistribution under these terms (or, alternatively, under the terms of the
ordinary General Public License).
To apply these terms, attach the following notices to the library. It is
safest to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least the
"copyright" line and a pointer to where the full notice is found.
<one line to give the library's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301
USA
Also add information on how to contact you by electronic and paper mail.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the library, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the
library `Frob' (a library for tweaking knobs) written by James Random
Hacker.
<signature of Ty Coon>, 1 April 1990
Ty Coon, President of Vice
That's all there is to it!

364
Localizations/Packages/slam_toolbox/README.md Normal file
View File

@@ -0,0 +1,364 @@
## Slam Toolbox
| DockerHub | [![Build Status](https://img.shields.io/docker/cloud/build/stevemacenski/slam-toolbox.svg?label=build)](https://hub.docker.com/r/stevemacenski/slam-toolbox) | [![Build Status](https://img.shields.io/docker/pulls/stevemacenski/slam-toolbox.svg?maxAge=2592000)](https://hub.docker.com/r/stevemacenski/slam-toolbox) |
|-----|----|----|
| **Build Farm** | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__slam_toolbox__ubuntu_bionic_amd64)](http://build.ros.org/view/Kbin_uX64/job/Mdev__slam_toolbox__ubuntu_bionic_amd64/) | N/A |
We've received feedback from users and have robots operating in the following environments with SLAM Toolbox:
- Retail
- Warehouses
- Libraries
- Research
### Cite This Work
You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783) and clicking on the image below.
> Macenski, S., Jambrecic I., "SLAM Toolbox: SLAM for the dynamic world", Journal of Open Source Software, 6(61), 2783, 2021.
> Macenski, S., "On Use of SLAM Toolbox, A fresh(er) look at mapping and localization for the dynamic world", ROSCon 2019.
[![IMAGE ALT TEXT](https://user-images.githubusercontent.com/14944147/74176653-f69beb80-4bec-11ea-906a-a233541a6064.png)](https://vimeo.com/378682207)
# Introduction
Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steven-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/) and in his free time.
This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes:
- Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps
- Continuing to refine, remap, or continue mapping a saved (serialized) pose-graph at any time
- life-long mapping: load a saved pose-graph continue mapping in a space while also removing extraneous information from newly added scans
- an optimization-based localization mode built on the pose-graph. Optionally run localization mode without a prior map for "lidar odometry" mode with local loop closures
- synchronous and asynchronous modes of mapping
- kinematic map merging (with an elastic graph manipulation merging technique in the works)
- plugin-based optimization solvers with a new optimized Google Ceres based plugin
- RVIZ plugin for interacting with the tools
- graph manipulation tools in RVIZ to manipulate nodes and connections during mapping
- Map serialization and lossless data storage
- ... more but those are the highlights
For running on live production robots, I recommend using the snap or from the build farm: slam-toolbox, it has optimizations in it that make it about 10x faster. You need the deb/source install for the other developer level tools that don't need to be on the robot (rviz plugins, etc).
This package has been benchmarked mapping building at 5x+ realtime up to about 30,000 sqft and 3x realtime up to about 60,000 sqft. with the largest area (I'm aware of) used was a 200,000 sq.ft. building in synchronous mode (e.i. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode.
The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. This data is currently available upon request, but its going to be included in a larger open-source dataset down the line.
![map_image](/images/circuit_launch.gif?raw=true "Map Image")
An overview of how the map was generated is presented below:
![slam_toolbox_sync_diagram](/images/slam_toolbox_sync.png)
1. ROS Node: SLAM toolbox is run in synchronous mode, which generates a ROS node. This node subscribes to laser scan and odometry topics, and publishes map to odom transform and a map.
2. Get odometry and LIDAR data: A callback for the laser topic will generate a pose (using odometry) and a laser scan tied at that node. These PosedScan objects form a queue, which are processed by the algorithm.
3. Process Data: The queue of PosedScan objects are used to construct a pose graph; odometry is refined using laser scan matching. This pose graph is used to compute robot pose, and find loop closures. If a loop closure is found, the pose graph is optimized, and pose estimates are updated. Pose estimates are used to compute and publish a map to odom transform for the robot.
4. Mapping: Laser scans associated with each pose in the pose graph are used to construct and publish a map.
# 03/23/2021 Note On Serialized Files
As of 03/23/2021, the contents of the serialized files has changed. For all new users after this date, this regard this section it does not impact you.
If you have previously existing serialized files (e.g. not `pgm` maps, but `.posegraph` serialized slam sessions), after this date, you may need to take some action to maintain current features. Unfortunately, an ABI breaking change was required to be made in order to fix a very large bug affecting any 360 or non-axially-mounted LIDAR system.
[This Discourse post](https://discourse.ros.org/t/request-for-input-potential-existing-slam-toolbox-serialized-file-invalidation/19520) highlights the issues. The frame storing the scan data for the optimizer was incorrect leading to explosions or flipping of maps for 360 and non-axially-aligned robots when using conservative loss functions. This change permanently fixes this issue, however it changes the frame of reference that this data is stored and serialized in. If your system as a non-360 lidar and it is mounted with its frame aligned with the robot base frame, you're unlikely to notice a problem and can disregard this statement. For all others noticing issues, you have the following options:
- Use the `<distro>-devel-unfixed` branch rather than `<distro>-devel`, which contains the unfixed version of this distribution's release which will be maintained in parallel to the main branches to have an option to continue with your working solution
- Convert your serialized files into the new reference frame with an offline utility
- Take the raw data and rerun the SLAM sessions to get a new serialized file with the right content
More of the conversation can be seen on tickets #198 and #281. I apologize for the inconvenience, however this solves a very large bug that was impacting a large number of users. I've worked hard to make sure there's a viable path forward for everyone.
# LifeLong Mapping
<!-- Continuing mapping Gif here-->
LifeLong mapping is the concept of being able to map a space, completely or partially, and over time, refine and update that map as you continue to interact with the space. Our approach implements this and also takes care to allow for the application of operating in the cloud, as well as mapping with many robots in a shared space (cloud distributed mapping). While Slam Toolbox can also just be used for a point-and-shoot mapping of a space and saving that map as a .pgm file as maps are traditionally stored in, it also allows you to save the pose-graph and metadata losslessly to reload later with the same or different robot and continue to map the space.
Our lifelong mapping consists of a few key steps
- Serialization and Deserialization to store and reload map information
- KD-Tree search matching to locate the robot in its position on reinitalization
- pose-graph optimizition based SLAM with 2D scan matching abstraction
This will allow the user to create and update existing maps, then serialize the data for use in other mapping sessions, something sorely lacking from most SLAM implementations and nearly all planar SLAM implementations. Other good libraries that do this include RTab-Map and Cartoprapher, though they themselves have their own quirks that make them (in my opinion) unusable for production robotics applications. This library provides the mechanics to save not only the data, but the pose graph, and associated metadata to work with. This has been used to create maps by merging techniques (taking 2 or more serialized objects and creating 1 globally consistent one) as well as continuous mapping techniques (updating 1, same, serialized map object over time and refining it). The major benefit of this over RTab-Map or Cartoprapher is the maturity of the underlying (but heavily modified) `open_karto` library the project is based on. The scan matcher of Karto is well known as an extremely good matcher for 2D laser scans and modified versions of Karto can be found in companies across the world.
Slam Toolbox supports all the major modes:
- Starting from a predefined dock (assuming to be near start region)
- Starting at any particular node - select a node ID to start near
- Starting in any particular area - indicate current pose in the map frame to start at, like AMCL
In the RVIZ interface (see section below) you'll be able to re-localize in a map or continue mapping graphically or programatically using ROS services.
On time of writing: there a **highly** experimental implementation of what I call "true lifelong" mapping that does support the method for removing nodes over time as well as adding nodes, this results in a true ability to map for life since the computation is bounded by removing extraneous or outdated information. Its recommended to run the non-full LifeLong mapping mode in the cloud for the increased computational burdens if you'd like to be continuously refining a map. However a real and desperately needed application of this is to have multi-session mapping to update just a section of the map or map half an area at a time to create a full (and then static) map for AMCL or Slam Toolbox localization mode, which this will handle in spades. The immediate plan is to create a mode within LifeLong mapping to decay old nodes to bound the computation and allow it to run on the edge by refining the experimental node. Continuing mapping (lifelong) should be used to build a complete map then switch to the pose-graph deformation localization mode until node decay is implemented, and **you should not see any substantial performance impacts**.
# Localization
<!-- map refind local area localization Gif here-->
Localization mode consists of 3 things:
- Loads existing serialized map into the node
- Maintains a rolling buffer of recent scans in the pose-graph
- After expiring from the buffer scans are removed and the underlying map is not affected
Localization methods on image map files has been around for years and works relatively well. There has not been a great deal of work in academia to refine these algorithms to a degree that satesfies me. However SLAM is a rich and well benchmarked topic. The inspiration of this work was the concept of "Can we make localization, SLAM again?" such that we can take advantage of all the nice things about SLAM for localization, but remove the unbounded computational increase.
To enable, set `mode: localization` in the configuration file to allow for the Ceres plugin to set itself correctly to be able to quickly add *and remove* nodes and constraints from the pose graph, but isn't strictly required, but a performance optimization. The localization mode will automatically load your pose graph, take the first scan and match it against the local area to further refine your estimated position, and start localizing.
To minimize the amount of changes required for moving to this mode over AMCL, we also expose a subscriber to the `/initialpose` topic used by AMCL to relocalize to a position, which also hooks up to the `2D Pose Estimation` tool in RVIZ. This way you can enter localization mode with our approach but continue to use the same API as you expect from AMCL for ease of integration.
In summary, this approach I dub `elastic pose-graph localization` is where we take existing map pose-graphs and localized with-in them with a rolling window of recent scans. This way we can localize in an existing map using the scan matcher, but not update the underlaying map long-term should something go wrong. It can be considered a replacement to AMCL and results is not needing any .pgm maps ever again. The lifelong mapping/continuous slam mode above will do better if you'd like to modify the underlying graph while moving.
## Tools
### Plugin based Optimizers
I have created a pluginlib interface for the ScanSolver abstract class so that you can change optimizers on runtime to test many different ones if you like.
Then I generated plugins for a few different solvers that people might be interested in. I like to swap them out for benchmarking and make sure its the same code running for all. I have supported Ceres, G2O, SPA, and GTSAM.
GTSAM/G2O/SPA is currently "unsupported" although all the code is there. They don't outperform Ceres settings I describe below so I stopped compiling them to save on build time, but they're there and work if you would like to use them. PRs to implement other optimizer plugins are welcome.
### Map Merging - Example uses of serialized raw data & posegraphs
#### Kinematic
This uses RVIZ and the plugin to load any number of posegraphs that will show up in RVIZ under `map_N` and a set of interactive markers to allow you to move them around. Once you have them all positioned relative to each other in the way you like, you can merge the submaps into a global `map` which can be downloaded with your map server implementation of choice.
It's more of a demonstration of other things you can do once you have the raw data to work with, but I don't suspect many people will get much use out of it unless you're used to stitching maps by hand.
More information in the RVIZ Plugin section below.
#### Pose Graph Merging
This is under development.
This is to solve the problem of merging many maps together with an initial guess of location in an elastic sense. This is something you just can't get if you don't have the full pose-graph and raw data to work with -- which we have from our continuous mapping work.
Hint: This is also really good for multi-robot map updating as well :)
### RVIZ Plugin
An rviz plugin is furnished to help with manual loop closures and online / offline mapping. By default interactive mode is off (allowing you to move nodes) as this takes quite a toll on rviz. When you want to move nodes, tick the interactive box, move what you want, and save changes to prompt a manual loop closure. Clear if you made a mistake. When done, exit interactive mode again.
There's also a tool to help you control online and offline data. You can at any time stop processing new scans or accepting new scans into the queue. This is desirable when you want to allow the package to catch up while the robot sits still (**This option is only meaningful in synchronous mode. In asynchronous mode the robot will never fall behind.**) or you want to stop processing new scans while you do a manual loop closure / manual "help". If there's more in the queue than you want, you may also clear it.
Additionally there's exposed buttons for the serialization and deserialization services to load an old pose-graph to update and refine, or continue mapping, then save back to file. The "Start By Dock" checkbox will try to scan match against the first node (assuming you started at your dock) to give you an odometry estimate to start with. Another option is to start using an inputted position in the GUI or by calling the underlying service. Additionally, you can use the current odometric position estimation if you happened to have just paused the robot or not moved much between runs. Finally (and most usefully), you can use the RVIZ tool for **2D Pose Estimation** to tell it where to go in **localization mode** just like AMCL.
Additionally the RVIZ plugin will allow you to add serialized map files as submaps in RVIZ. They will be displayed with an interactive marker you can translate and rotate to match up, then generate a composite map with the Generate Map button. At that point the composite map is being broadcasted on the `/map` topic and you can save it with the `map_saver`.
It's recommended to always continue mapping near the dock, if that's not possible, look into the starting from pose or map merging techniques. This RVIZ plugin is mostly here as a debug utility, but if you often find yourself mapping areas using rviz already, I'd just have it open. All the RVIZ buttons are implemented using services that a master application can control.
The interface is shown below.
![rviz_plugin](/images/rviz_plugin.png?raw=true "Rviz Plugin")
### Graph Manipulation
By enabling `Interactive Mode`, the graph nodes will change from markers to interactive markers which you can manipulate. When you move a node(s), you can Save Changes and it will send the updated position to the pose-graph and cause an optimization run to occur to change the pose-graph with your new node location. This is helpful if the robot gets pushed, slips, runs into a wall, or otherwise has drifting odometry and you would like to manually correct it.
When a map is sufficiently large, the number of interactive markers in RVIZ may be too large and RVIZ may start to lag. I only recommend using this feature as a testing debug tool and not for production. However if you are able to make it work with 10,000 interactive markers, I'll merge that PR in a heartbeat. Otherwise I'd restrict the use of this feature to small maps or with limited time to make a quick change and return to static mode by unchecking the box.
## Metrics
If you're a weirdo like me and you want to see how I came up with the settings I had for the Ceres optimizer, see below.
![ceres_solver_comparison](https://user-images.githubusercontent.com/14944147/41576505-a6802d76-733c-11e8-8eca-334da2c8bd50.png)
The data sets present solve time vs number of nodes in the pose graph on a large dataset, as that is not open source, but suffice to say that the settings I recommend work well. I think anyone would be hardset in a normal application to exceed or find that another solver type is better (that super low curve on the bottom one, yeah, that's it). Benchmark on a low power 7th gen i7 machine.
It can map _very_ large spaces with reasonable CPU and memory consumption. My default settings increase O(N) on number of elements in the pose graph. I recommend from extensive testing to use the `SPARSE_NORMAL_CHOLESKY` solver with Ceres and the `SCHUR_JACOBI` preconditioner. Using `LM` at the trust region strategy is comparable to the dogleg subspace strategy, but `LM` is much better supported so why argue with it.
You can get away without a loss function if your odometry is good (ie likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server.
# API
The following are the services/topics that are exposed for use. See the rviz plugin for an implementation of their use.
## Subscribed topics
| scan | `sensor_msgs/LaserScan` | the input scan from your laser to utilize |
|-----|----|----|
| **tf** | N/A | a valid transform from your configured odom_frame to base_frame |
## Published topics
| Topic | Type | Description |
|-----|----|----|
| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency |
| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match |
## Exposed Services
| Topic | Type | Description |
|-----|----|----|
| `/slam_toolbox/clear_changes` | `slam_toolbox/Clear` | Clear all manual pose-graph manipulation changes pending |
| `/slam_toolbox/deserialize_map` | `slam_toolbox/DeserializePoseGraph` | Load a saved serialized pose-graph files from disk |
| `/slam_toolbox/dynamic_map` | `nav_msgs/OccupancyGrid` | Request the current state of the pose-graph as an occupancy grid |
| `/slam_toolbox/manual_loop_closure` | `slam_toolbox/LoopClosure` | Request the manual changes to the pose-graph pending to be processed |
| `/slam_toolbox/pause_new_measurements` | `slam_toolbox/Pause` | Pause processing of new incoming laser scans by the toolbox |
| `/slam_toolbox/save_map` | `slam_toolbox/SaveMap` | Save the map image file of the pose-graph that is useable for display or AMCL localization. It is a simple wrapper on `map_server/map_saver` but is useful. |
| `/slam_toolbox/serialize_map` | `slam_toolbox/SerializePoseGraph` | Save the map pose-graph and datathat is useable for continued mapping, slam_toolbox localization, offline manipulation, and more |
| `/slam_toolbox/toggle_interactive_mode` | `slam_toolbox/ToggleInteractive` | Toggling in and out of interactive mode, publishing interactive markers of the nodes and their positions to be updated in an application |
# Configuration
The following settings and options are exposed to you. My default configuration is given in `config` directory.
## Solver Params
`solver_plugin` - The type of nonlinear solver to utilize for karto's scan solver. Options: `solver_plugins::CeresSolver`, `solver_plugins::SpaSolver`, `solver_plugins::G2oSolver`. Default: `solver_plugins::CeresSolver`.
`ceres_linear_solver` - The linear solver for Ceres to use. Options: `SPARSE_NORMAL_CHOLESKY`, `SPARSE_SCHUR`, `ITERATIVE_SCHUR`, `CGNR`. Defaults to `SPARSE_NORMAL_CHOLESKY`.
`ceres_preconditioner` - The preconditioner to use with that solver. Options: `JACOBI`, `IDENTITY` (none), `SCHUR_JACOBI`. Defaults to `JACOBI`.
`ceres_trust_strategy` - The trust region strategy. Line searach strategies are not exposed because they perform poorly for this use. Options: `LEVENBERG_MARQUARDT`, `DOGLEG`. Default: `LEVENBERG_MARQUARDT`.
`ceres_dogleg_type` - The dogleg strategy to use if the trust strategy is `DOGLEG`. Options: `TRADITIONAL_DOGLEG`, `SUBSPACE_DOGLEG`. Default: `TRADITIONAL_DOGLEG`
`ceres_loss_function` - The type of loss function to reject outlier measurements. None is equatable to a squared loss. Options: `None`, `HuberLoss`, `CauchyLoss`. Default: `None`.
`mode` - "mapping" or "localization" mode for performance optimizations in the Ceres problem creation
## Toolbox Params
`odom_frame` - Odometry frame
`map_frame` - Map frame
`base_frame` - Base frame
`map_file_name` - Name of the pose-graph file to load on startup if available
`map_start_pose` - Pose to start pose-graph mapping/localization in, if available
`map_start_at_dock` - Starting pose-graph loading at the dock (first node), if available. If both pose and dock are set, it will use pose
`debug_logging` - Change logger to debug
`throttle_scans` - Number of scans to throttle in synchronous mode
`transform_publish_period` - The map to odom transform publish period. 0 will not publish transforms
`map_update_interval` - Interval to update the 2D occupancy map for other applications / visualization
`enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes.
`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match. This can be used to tune the influence of the pose position in a downstream localization filter. The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters. Default: 1.0
`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match. See description of position_covariance_scale. Default: 1.0
`resolution` - Resolution of the 2D occupancy map to generate
`max_laser_range` - Maximum laser range to use for 2D occupancy map rastering
`minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode
`transform_timeout` - TF timeout for looking up transforms
`tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode.
`stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine.
`minimum_travel_distance` - Minimum distance of travel before processing a new scan
## Matcher Params
`use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?)
`use_scan_barycenter` - Whether to use the barycenter or scan pose
`minimum_travel_heading` - Minimum changing in heading to justify an update
`scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode
`scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer
`link_match_minimum_response_fine` - The threshold link matching algorithm response for fine resolution to pass
`link_scan_maximum_distance` - Maximum distance between linked scans to be valid
`loop_search_maximum_distance` - Maximum threshold of distance for scans to be considered for loop closure
`do_loop_closing` - Whether to do loop closure (if you're not sure, the answer is "true")
`loop_match_minimum_chain_size` - The minimum chain length of scans to look for loop closure
`loop_match_maximum_variance_coarse` - The threshold variance in coarse search to pass to refine
`loop_match_minimum_response_coarse` - The threshold response of the loop closure algorithm in coarse search to pass to refine
`loop_match_minimum_response_fine` - The threshold response of the loop closure algorithm in fine search to pass to refine
`correlation_search_space_dimension` - Search grid size to do scan correlation over
`correlation_search_space_resolution` - Search grid resolution to do scan correlation over
`correlation_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses
`loop_search_space_dimension` - Size of the search grid over the loop closure algorith
`loop_search_space_resolution` - Search grid resolution to do loop closure over
`loop_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses
`distance_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose
`angle_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose
`fine_search_angle_offset` - Range of angles to test for fine scan matching
`coarse_search_angle_offset` - Range of angles to test for coarse scan matching
`coarse_angle_resolution` - Resolution of angles over the Offset range to test in scan matching
`minimum_angle_penalty` - Smallest penalty an angle can have to ensure the size doesn't blow up
`minimum_distance_penalty` - Smallest penalty a scan can have to ensure the size doesn't blow up
`use_response_expansion` - Whether to automatically increase the search grid size if no viable match is found
# Install
ROSDep will take care of the major things
```
rosdep install -q -y -r --from-paths src --ignore-src
```
Also released in Melodic / Dashing to the ROS build farm to install debians.
Run your catkin build procedure of choice.
You can run via `roslaunch slam_toolbox online_sync.launch`
# Etc
## NanoFlann!
In order to do some operations quickly for continued mapping and localization, I make liberal use of NanoFlann (shout out!).
## Brief incursion into snaps
Snap are completely isolated containerized packages that one can run through the Canonical organization on a large number of Linux distributions. They're similar to Docker containers but it doesn't share the kernel or any of the libraries, and rather has everything internal as essentially a seperate partitioned operating system based on Ubuntu Core.
We package up slam toolbox in this way for a nice multiple-on speed up in execution from a couple of pretty nuanced reasons in this particular project, but generally speaking you shouldn't expect a speedup from a snap. There's a generate snap script in the `snap` directory to create a snap.
Since Snaps are totally isolated and there's no override flags like in Docker, there's only a couple of fixed directories that both the snap and the host system can write and read from, including SNAP_COMMON (usually in `/var/snap/[snap name]/common`). Therefore, this is the place that if you're serializing and deserializing maps, you need to have them accessible to that directory.
You can optionally store all your serialized maps there, move maps there as needed, take maps from there after serialization, or do my favorite option and `link` the directories with `ln` to where ever you normally store your maps and you're wanting to dump your serialized map files.
Example of `ln`:
```
# Source Linked
sudo ln -s /home/steve/maps/serialized_map/ /var/snap/slam-toolbox/common
```
and then all you have to do when you specify a map to use is set the filename to `slam-toolbox/map_name` and it should work no matter if you're running in a snap, docker, or on bare metal. The `-s` makes a symbol link so rather than `/var/snap/slam-toolbox/common/*` containing the maps, `/var/snap/slam-toolbox/common/serialized_map/*` will. By default on bare metal, the maps will be saved in `.ros`
## More Gifs!
![map_image](/images/mapping_steves_apartment.gif?raw=true "Map Image")
If someone from iRobot can use this to tell me my Roomba serial number by correlating to its maps, I'll buy them lunch and probably try to hire them.

1
Localizations/Packages/slam_toolbox/slam_toolbox/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
snap_ws/*

25
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCSparse.cmake Normal file
View File

@@ -0,0 +1,25 @@
# Look for csparse; note the difference in the directory specifications!
FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
PATHS
/usr/include/suitesparse
/usr/include
/opt/local/include
/usr/local/include
/sw/include
/usr/include/ufsparse
/opt/local/include/ufsparse
/usr/local/include/ufsparse
/sw/include/ufsparse
)
FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(CSPARSE DEFAULT_MSG
CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

89
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCholmod.cmake Normal file
View File

@@ -0,0 +1,89 @@
# Cholmod lib usually requires linking to a blas and lapack library.
# It is up to the user of this module to find a BLAS and link to it.
if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
set(CHOLMOD_FIND_QUIETLY TRUE)
endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
find_path(CHOLMOD_INCLUDE_DIR
NAMES
cholmod.h
PATHS
$ENV{CHOLMODDIR}
${INCLUDE_INSTALL_DIR}
PATH_SUFFIXES
suitesparse
ufsparse
)
find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY})
if(CHOLMOD_LIBRARIES)
get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (AMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (COLAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CCOLAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CHOLMOD_METIS_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (SUITESPARSECONFIG_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY})
endif ()
endif(CHOLMOD_LIBRARIES)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
mark_as_advanced(CHOLMOD_LIBRARIES)

122
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindG2O.cmake Normal file
View File

@@ -0,0 +1,122 @@
# Find the header files
FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
${G2O_ROOT}/include
$ENV{G2O_ROOT}/include
$ENV{G2O_ROOT}
/usr/local/include
/usr/include
/opt/local/include
/sw/local/include
/sw/include
/opt/ros/indigo/include
/opt/ros/jade/include
/opt/ros/kinetic/include
NO_DEFAULT_PATH
)
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.
MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
${G2O_ROOT}/lib/Debug
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Debug
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
/opt/ros/indigo/lib
/opt/ros/jade/lib
/opt/ros/kinetic/lib
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
${G2O_ROOT}/lib/Release
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Release
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
/opt/ros/indigo/lib
/opt/ros/jade/lib
/opt/ros/kinetic/lib
)
IF(NOT ${MYLIBRARY}_DEBUG)
IF(MYLIBRARY)
SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
ENDIF(MYLIBRARY)
ENDIF( NOT ${MYLIBRARY}_DEBUG)
ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
# G2O solvers declared found if we found at least one solver
SET(G2O_SOLVERS_FOUND "NO")
IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
SET(G2O_SOLVERS_FOUND "YES")
ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
# G2O itself declared found if we found the core libraries and at least one solver
SET(G2O_FOUND "NO")
IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
SET(G2O_FOUND "YES")
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

85
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindGTSAM.cmake Normal file
View File

@@ -0,0 +1,85 @@
# This is FindGTSAM.cmake
# CMake module to locate the GTSAM package
#
# The following cache variables may be set before calling this script:
#
# GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam)
# GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory
# within it (e.g build-debug). Without this defined, this script tries to
# intelligently find the build directory based on the project's build directory name
# or based on the build type (Debug/Release/etc).
#
# The following variables will be defined:
#
# GTSAM_FOUND : TRUE if the package has been successfully found
# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories
# GTSAM_LIBS : paths to GTSAM's libraries
#
# NOTES on compiling against an uninstalled GTSAM build tree:
# - A GTSAM source tree will be automatically searched for in the directory
# 'gtsam' next to your project directory, after searching
# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr.
# - The build directory will be searched first with the same name as your
# project's build directory, e.g. if you build from 'MyProject/build-optimized',
# 'gtsam/build-optimized' will be searched first. Next, a build directory for
# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is
# 'Release', then 'gtsam/build-release' will be searched next. Finally, plain
# 'gtsam/build' will be searched.
# - You can control the gtsam build directory name directly by defining the CMake
# cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will
# be searched.
# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam
# directory.
# Get path suffixes to help look for gtsam
if(GTSAM_BUILD_NAME)
set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam")
else()
# lowercase build type
string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix)
# build suffix of this project
get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME)
set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam")
endif()
# Use GTSAM_ROOT or GTSAM_DIR equivalently
if(GTSAM_ROOT AND NOT GTSAM_DIR)
set(GTSAM_DIR "${GTSAM_ROOT}")
endif()
if(GTSAM_DIR)
# Find include dirs
find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h
PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH
DOC "GTSAM include directories")
# Find libraries
find_library(GTSAM_LIBS NAMES gtsam
HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH
PATH_SUFFIXES ${gtsam_build_names}
DOC "GTSAM libraries")
else()
# Find include dirs
set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include)
find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h
PATHS ${extra_include_paths}
DOC "GTSAM include directories")
if(NOT GTSAM_INCLUDE_DIR)
message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}")
endif()
# Find libraries
find_library(GTSAM_LIBS NAMES gtsam
HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib
PATH_SUFFIXES ${gtsam_build_names}
DOC "GTSAM libraries")
endif()
# handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE
# if all listed variables are TRUE
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(GTSAM DEFAULT_MSG
GTSAM_LIBS GTSAM_INCLUDE_DIR)

250
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindMKL.cmake Normal file
View File

@@ -0,0 +1,250 @@
# This file is adapted from the one in OpenMEEG: http://www-sop.inria.fr/athena/software/OpenMEEG/
# - Try to find the Intel Math Kernel Library
# Once done this will define
#
# MKL_FOUND - system has MKL
# MKL_ROOT_DIR - path to the MKL base directory
# MKL_INCLUDE_DIR - the MKL include directory
# MKL_LIBRARIES - MKL libraries
#
# There are few sets of libraries:
# Array indexes modes:
# LP - 32 bit indexes of arrays
# ILP - 64 bit indexes of arrays
# Threading:
# SEQUENTIAL - no threading
# INTEL - Intel threading library
# GNU - GNU threading library
# MPI support
# NOMPI - no MPI support
# INTEL - Intel MPI library
# OPEN - Open MPI library
# SGI - SGI MPT Library
# linux
IF(UNIX AND NOT APPLE)
IF(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64")
SET(MKL_ARCH_DIR "intel64")
ELSE()
SET(MKL_ARCH_DIR "32")
ENDIF()
ENDIF()
# macos
IF(APPLE)
SET(MKL_ARCH_DIR "intel64")
ENDIF()
IF(FORCE_BUILD_32BITS)
set(MKL_ARCH_DIR "32")
ENDIF()
# windows
IF(WIN32)
IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8)
SET(MKL_ARCH_DIR "intel64")
ELSE()
SET(MKL_ARCH_DIR "ia32")
ENDIF()
ENDIF()
SET(MKL_THREAD_VARIANTS SEQUENTIAL GNUTHREAD INTELTHREAD)
SET(MKL_MODE_VARIANTS ILP LP)
SET(MKL_MPI_VARIANTS NOMPI INTELMPI OPENMPI SGIMPT)
FIND_PATH(MKL_ROOT_DIR
include/mkl_cblas.h
PATHS
$ENV{MKLDIR}
/opt/intel/mkl/
/opt/intel/mkl/*/
/opt/intel/cmkl/
/opt/intel/cmkl/*/
/opt/intel/*/mkl/
/Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
"C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
"C:/Program Files (x86)/Intel/Composer XE 2013/mkl"
"C:/Program Files/Intel/MKL/*/"
"C:/Program Files/Intel/ComposerXE-2011/mkl"
"C:/Program Files/Intel/Composer XE 2013/mkl"
)
FIND_PATH(MKL_INCLUDE_DIR
mkl_cblas.h
PATHS
${MKL_ROOT_DIR}/include
${INCLUDE_INSTALL_DIR}
)
FIND_PATH(MKL_FFTW_INCLUDE_DIR
fftw3.h
PATH_SUFFIXES fftw
PATHS
${MKL_ROOT_DIR}/include
${INCLUDE_INSTALL_DIR}
NO_DEFAULT_PATH
)
IF(WIN32)
SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}")
IF (MKL_INCLUDE_DIR MATCHES "10.")
IF(CMAKE_CL_64)
SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64)
ELSE()
SET(MKL_LIBS mkl_solver mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libguide mkl_lapack95 mkl_blas95)
ENDIF()
ELSEIF(MKL_INCLUDE_DIR MATCHES "2013") # version 11 ...
IF(CMAKE_CL_64)
SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_intel_thread libiomp5md mkl_lapack95_lp64 mkl_blas95_lp64)
ELSE()
SET(MKL_LIBS mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libiomp5md mkl_lapack95 mkl_blas95)
ENDIF()
ELSE() # old MKL 9
SET(MKL_LIBS mkl_solver mkl_c libguide mkl_lapack mkl_ia32)
ENDIF()
IF (MKL_INCLUDE_DIR MATCHES "10.3")
SET(MKL_LIBS ${MKL_LIBS} libiomp5md)
ENDIF()
FOREACH (LIB ${MKL_LIBS})
FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH)
IF(${LIB}_PATH)
SET(MKL_LIBRARIES ${MKL_LIBRARIES} ${${LIB}_PATH})
ELSE()
MESSAGE(STATUS "Could not find ${LIB}: disabling MKL")
ENDIF()
ENDFOREACH()
SET(MKL_FOUND ON)
ELSE() # UNIX and macOS
FIND_LIBRARY(MKL_CORE_LIBRARY
mkl_core
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
# Threading libraries
FIND_LIBRARY(MKL_SEQUENTIAL_LIBRARY
mkl_sequential
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
FIND_LIBRARY(MKL_INTELTHREAD_LIBRARY
mkl_intel_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
IF(NOT APPLE)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Intel Libraries
IF("${MKL_ARCH_DIR}" STREQUAL "32")
FIND_LIBRARY(MKL_LP_LIBRARY
mkl_intel
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
FIND_LIBRARY(MKL_ILP_LIBRARY
mkl_intel
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
else()
FIND_LIBRARY(MKL_LP_LIBRARY
mkl_intel_lp64
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
FIND_LIBRARY(MKL_ILP_LIBRARY
mkl_intel_ilp64
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Lapack
FIND_LIBRARY(MKL_LAPACK_LIBRARY
mkl_lapack
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
IF(NOT MKL_LAPACK_LIBRARY)
FIND_LIBRARY(MKL_LAPACK_LIBRARY
mkl_lapack95_lp64
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# iomp5
IF("${MKL_ARCH_DIR}" STREQUAL "32")
IF(UNIX AND NOT APPLE)
FIND_LIBRARY(MKL_IOMP5_LIBRARY
iomp5
PATHS
${MKL_ROOT_DIR}/../lib/ia32
)
ELSE()
SET(MKL_IOMP5_LIBRARY "") # no need for mac
ENDIF()
else()
IF(UNIX AND NOT APPLE)
FIND_LIBRARY(MKL_IOMP5_LIBRARY
iomp5
PATHS
${MKL_ROOT_DIR}/../lib/intel64
)
ELSE()
SET(MKL_IOMP5_LIBRARY "") # no need for mac
ENDIF()
ENDIF()
foreach (MODEVAR ${MKL_MODE_VARIANTS})
foreach (THREADVAR ${MKL_THREAD_VARIANTS})
if (MKL_CORE_LIBRARY AND MKL_${MODEVAR}_LIBRARY AND MKL_${THREADVAR}_LIBRARY)
set(MKL_${MODEVAR}_${THREADVAR}_LIBRARIES
${MKL_${MODEVAR}_LIBRARY} ${MKL_${THREADVAR}_LIBRARY} ${MKL_CORE_LIBRARY}
${MKL_LAPACK_LIBRARY} ${MKL_IOMP5_LIBRARY})
# message("${MODEVAR} ${THREADVAR} ${MKL_${MODEVAR}_${THREADVAR}_LIBRARIES}") # for debug
endif()
endforeach()
endforeach()
IF(APPLE)
SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF()
INCLUDE(FindPackageHandleStandardArgs)
find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES)
#if(MKL_FOUND)
# LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack
#endif()
MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)

127
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindSuiteSparse.cmake Normal file
View File

@@ -0,0 +1,127 @@
FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h
PATHS
${SUITE_SPARSE_ROOT}/include
/usr/include/suitesparse
/usr/include/ufsparse
/opt/local/include/ufsparse
/usr/local/include/ufsparse
/sw/include/ufsparse
)
FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(CAMD_LIBRARY NAMES camd
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
# Different platforms seemingly require linking against different sets of libraries
IF(CYGWIN)
FIND_PACKAGE(PkgConfig)
FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
PKG_CHECK_MODULES(LAPACK lapack)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES})
# MacPorts build of the SparseSuite requires linking against extra libraries
ELSEIF(APPLE)
FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(METIS_LIBRARY NAMES metis
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate")
ELSE(APPLE)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY})
ENDIF(CYGWIN)
IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
SET(CHOLMOD_FOUND TRUE)
ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
SET(CHOLMOD_FOUND FALSE)
ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
# Look for csparse; note the difference in the directory specifications!
FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
PATHS
/usr/include/suitesparse
/usr/include
/opt/local/include
/usr/local/include
/sw/include
/usr/include/ufsparse
/opt/local/include/ufsparse
/usr/local/include/ufsparse
/sw/include/ufsparse
)
FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
SET(CSPARSE_FOUND TRUE)
ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
SET(CSPARSE_FOUND FALSE)
ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)

421
Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindTBB.cmake Normal file
View File

@@ -0,0 +1,421 @@
# - Find ThreadingBuildingBlocks include dirs and libraries
# Use this module by invoking find_package with the form:
# find_package(TBB
# [REQUIRED] # Fail with error if TBB is not found
# ) #
# Once done, this will define
#
# TBB_FOUND - system has TBB
# TBB_INCLUDE_DIRS - the TBB include directories
# TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or
# malloc proxy
# TBB::tbb - imported target for the TBB library
#
# TBB_VERSION_MAJOR - Major Product Version Number
# TBB_VERSION_MINOR - Minor Product Version Number
# TBB_INTERFACE_VERSION - Engineering Focused Version Number
# TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version
# still supported. This uses the engineering
# focused interface version numbers.
#
# TBB_MALLOC_FOUND - system has TBB malloc library
# TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories
# TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined
# TBB::malloc - imported target for the TBB malloc library
#
# TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library
# TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories
# TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined
# TBB::malloc_proxy - imported target for the TBB malloc proxy library
#
#
# This module reads hints about search locations from variables:
# ENV TBB_ARCH_PLATFORM - for eg. set it to "mic" for Xeon Phi builds
# ENV TBB_ROOT or just TBB_ROOT - root directory of tbb installation
# ENV TBB_BUILD_PREFIX - specifies the build prefix for user built tbb
# libraries. Should be specified with ENV TBB_ROOT
# and optionally...
# ENV TBB_BUILD_DIR - if build directory is different than ${TBB_ROOT}/build
#
#
# Modified by Robert Maynard from the original OGRE source
#
#-------------------------------------------------------------------
# This file is part of the CMake build system for OGRE
# (Object-oriented Graphics Rendering Engine)
# For the latest info, see http://www.ogre3d.org/
#
# The contents of this file are placed in the public domain. Feel
# free to make use of it in any way you like.
#-------------------------------------------------------------------
#
#=============================================================================
# Copyright 2010-2012 Kitware, Inc.
# Copyright 2012 Rolf Eike Beer <eike@sf-mail.de>
#
# Distributed under the OSI-approved BSD License (the "License");
# see accompanying file Copyright.txt for details.
#
# This software is distributed WITHOUT ANY WARRANTY; without even the
# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the License for more information.
#=============================================================================
# (To distribute this file outside of CMake, substitute the full
# License text for the above reference.)
#=============================================================================
# FindTBB helper functions and macros
#
#====================================================
# Fix the library path in case it is a linker script
#====================================================
function(tbb_extract_real_library library real_library)
if(NOT UNIX OR NOT EXISTS ${library})
set(${real_library} "${library}" PARENT_SCOPE)
return()
endif()
#Read in the first 4 bytes and see if they are the ELF magic number
set(_elf_magic "7f454c46")
file(READ ${library} _hex_data OFFSET 0 LIMIT 4 HEX)
if(_hex_data STREQUAL _elf_magic)
#we have opened a elf binary so this is what
#we should link to
set(${real_library} "${library}" PARENT_SCOPE)
return()
endif()
file(READ ${library} _data OFFSET 0 LIMIT 1024)
if("${_data}" MATCHES "INPUT \\(([^(]+)\\)")
#extract out the .so name from REGEX MATCH command
set(_proper_so_name "${CMAKE_MATCH_1}")
#construct path to the real .so which is presumed to be in the same directory
#as the input file
get_filename_component(_so_dir "${library}" DIRECTORY)
set(${real_library} "${_so_dir}/${_proper_so_name}" PARENT_SCOPE)
else()
#unable to determine what this library is so just hope everything works
#and pass it unmodified.
set(${real_library} "${library}" PARENT_SCOPE)
endif()
endfunction()
#===============================================
# Do the final processing for the package find.
#===============================================
macro(findpkg_finish PREFIX TARGET_NAME)
# skip if already processed during this run
if (NOT ${PREFIX}_FOUND)
if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY)
set(${PREFIX}_FOUND TRUE)
set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR})
set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY})
else ()
if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY)
message(FATAL_ERROR "Required library ${PREFIX} not found.")
endif ()
endif ()
if (NOT TARGET "TBB::${TARGET_NAME}")
if (${PREFIX}_LIBRARY_RELEASE)
tbb_extract_real_library(${${PREFIX}_LIBRARY_RELEASE} real_release)
endif ()
if (${PREFIX}_LIBRARY_DEBUG)
tbb_extract_real_library(${${PREFIX}_LIBRARY_DEBUG} real_debug)
endif ()
add_library(TBB::${TARGET_NAME} UNKNOWN IMPORTED)
set_target_properties(TBB::${TARGET_NAME} PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES "${${PREFIX}_INCLUDE_DIR}")
if (${PREFIX}_LIBRARY_DEBUG AND ${PREFIX}_LIBRARY_RELEASE)
set_target_properties(TBB::${TARGET_NAME} PROPERTIES
IMPORTED_LOCATION "${real_release}"
IMPORTED_LOCATION_DEBUG "${real_debug}"
IMPORTED_LOCATION_RELEASE "${real_release}")
elseif (${PREFIX}_LIBRARY_RELEASE)
set_target_properties(TBB::${TARGET_NAME} PROPERTIES
IMPORTED_LOCATION "${real_release}")
elseif (${PREFIX}_LIBRARY_DEBUG)
set_target_properties(TBB::${TARGET_NAME} PROPERTIES
IMPORTED_LOCATION "${real_debug}")
endif ()
endif ()
#mark the following variables as internal variables
mark_as_advanced(${PREFIX}_INCLUDE_DIR
${PREFIX}_LIBRARY
${PREFIX}_LIBRARY_DEBUG
${PREFIX}_LIBRARY_RELEASE)
endif ()
endmacro()
#===============================================
# Generate debug names from given release names
#===============================================
macro(get_debug_names PREFIX)
foreach(i ${${PREFIX}})
set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i})
endforeach()
endmacro()
#===============================================
# See if we have env vars to help us find tbb
#===============================================
macro(getenv_path VAR)
set(ENV_${VAR} $ENV{${VAR}})
# replace won't work if var is blank
if (ENV_${VAR})
string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} )
endif ()
endmacro()
#===============================================
# Couple a set of release AND debug libraries
#===============================================
macro(make_library_set PREFIX)
if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG)
set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG})
elseif (${PREFIX}_RELEASE)
set(${PREFIX} ${${PREFIX}_RELEASE})
elseif (${PREFIX}_DEBUG)
set(${PREFIX} ${${PREFIX}_DEBUG})
endif ()
endmacro()
#=============================================================================
# Now to actually find TBB
#
# Get path, convert backslashes as ${ENV_${var}}
getenv_path(TBB_ROOT)
# initialize search paths
set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT})
set(TBB_INC_SEARCH_PATH "")
set(TBB_LIB_SEARCH_PATH "")
# If user built from sources
set(TBB_BUILD_PREFIX $ENV{TBB_BUILD_PREFIX})
if (TBB_BUILD_PREFIX AND ENV_TBB_ROOT)
getenv_path(TBB_BUILD_DIR)
if (NOT ENV_TBB_BUILD_DIR)
set(ENV_TBB_BUILD_DIR ${ENV_TBB_ROOT}/build)
endif ()
# include directory under ${ENV_TBB_ROOT}/include
list(APPEND TBB_LIB_SEARCH_PATH
${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_release
${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_debug)
endif ()
# For Windows, let's assume that the user might be using the precompiled
# TBB packages from the main website. These use a rather awkward directory
# structure (at least for automatically finding the right files) depending
# on platform and compiler, but we'll do our best to accommodate it.
# Not adding the same effort for the precompiled linux builds, though. Those
# have different versions for CC compiler versions and linux kernels which
# will never adequately match the user's setup, so there is no feasible way
# to detect the "best" version to use. The user will have to manually
# select the right files. (Chances are the distributions are shipping their
# custom version of tbb, anyway, so the problem is probably nonexistent.)
if (WIN32 AND MSVC)
set(COMPILER_PREFIX "vc7.1")
if (MSVC_VERSION EQUAL 1400)
set(COMPILER_PREFIX "vc8")
elseif(MSVC_VERSION EQUAL 1500)
set(COMPILER_PREFIX "vc9")
elseif(MSVC_VERSION EQUAL 1600)
set(COMPILER_PREFIX "vc10")
elseif(MSVC_VERSION EQUAL 1700)
set(COMPILER_PREFIX "vc11")
elseif(MSVC_VERSION EQUAL 1800)
set(COMPILER_PREFIX "vc12")
elseif(MSVC_VERSION EQUAL 1900)
set(COMPILER_PREFIX "vc14")
endif ()
# for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path
foreach (dir IN LISTS TBB_PREFIX_PATH)
if (CMAKE_CL_64)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX})
else ()
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX})
endif ()
endforeach ()
endif ()
# For OS X binary distribution, choose libc++ based libraries for Mavericks (10.9)
# and above and AppleClang
if (CMAKE_SYSTEM_NAME STREQUAL "Darwin" AND
NOT CMAKE_SYSTEM_VERSION VERSION_LESS 13.0)
set (USE_LIBCXX OFF)
cmake_policy(GET CMP0025 POLICY_VAR)
if (POLICY_VAR STREQUAL "NEW")
if (CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
set (USE_LIBCXX ON)
endif ()
else ()
if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
set (USE_LIBCXX ON)
endif ()
endif ()
if (USE_LIBCXX)
foreach (dir IN LISTS TBB_PREFIX_PATH)
list (APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/libc++ ${dir}/libc++/lib)
endforeach ()
endif ()
endif ()
# check compiler ABI
if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(COMPILER_PREFIX)
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.7)
list(APPEND COMPILER_PREFIX "gcc4.7")
endif()
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.4)
list(APPEND COMPILER_PREFIX "gcc4.4")
endif()
list(APPEND COMPILER_PREFIX "gcc4.1")
elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
set(COMPILER_PREFIX)
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.6)
list(APPEND COMPILER_PREFIX "gcc4.7")
endif()
list(APPEND COMPILER_PREFIX "gcc4.4")
else() # Assume compatibility with 4.4 for other compilers
list(APPEND COMPILER_PREFIX "gcc4.4")
endif ()
# if platform architecture is explicitly specified
set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM})
if (TBB_ARCH_PLATFORM)
foreach (dir IN LISTS TBB_PREFIX_PATH)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM})
endforeach ()
endif ()
foreach (dir IN LISTS TBB_PREFIX_PATH)
foreach (prefix IN LISTS COMPILER_PREFIX)
if (CMAKE_SIZEOF_VOID_P EQUAL 8)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${prefix})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${prefix}/lib)
else ()
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${prefix})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${prefix}/lib)
endif ()
endforeach()
endforeach ()
# add general search paths
foreach (dir IN LISTS TBB_PREFIX_PATH)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib ${dir}/Lib ${dir}/lib/tbb
${dir}/Libs)
list(APPEND TBB_INC_SEARCH_PATH ${dir}/include ${dir}/Include
${dir}/include/tbb)
endforeach ()
set(TBB_LIBRARY_NAMES tbb)
get_debug_names(TBB_LIBRARY_NAMES)
find_path(TBB_INCLUDE_DIR
NAMES tbb/tbb.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_LIBRARY_RELEASE
NAMES ${TBB_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_LIBRARY_DEBUG
NAMES ${TBB_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_LIBRARY)
findpkg_finish(TBB tbb)
#if we haven't found TBB no point on going any further
if (NOT TBB_FOUND)
return()
endif ()
#=============================================================================
# Look for TBB's malloc package
set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc)
get_debug_names(TBB_MALLOC_LIBRARY_NAMES)
find_path(TBB_MALLOC_INCLUDE_DIR
NAMES tbb/tbb.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_MALLOC_LIBRARY_RELEASE
NAMES ${TBB_MALLOC_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_MALLOC_LIBRARY_DEBUG
NAMES ${TBB_MALLOC_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_MALLOC_LIBRARY)
findpkg_finish(TBB_MALLOC tbbmalloc)
#=============================================================================
# Look for TBB's malloc proxy package
set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy)
get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES)
find_path(TBB_MALLOC_PROXY_INCLUDE_DIR
NAMES tbb/tbbmalloc_proxy.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_MALLOC_PROXY_LIBRARY_RELEASE
NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_MALLOC_PROXY_LIBRARY_DEBUG
NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_MALLOC_PROXY_LIBRARY)
findpkg_finish(TBB_MALLOC_PROXY tbbmalloc_proxy)
#=============================================================================
#parse all the version numbers from tbb
if(NOT TBB_VERSION)
#only read the start of the file
file(STRINGS
"${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h"
TBB_VERSION_CONTENTS
REGEX "VERSION")
string(REGEX REPLACE
".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
endif()

173
Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt Normal file
View File

@@ -0,0 +1,173 @@
cmake_minimum_required(VERSION 3.0.2)
project(slam_toolbox)
add_compile_options(-std=c++17)
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/CMake/")
list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/karto_sdk/cmake)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive -std=c++17")
#karto_sdk lib
set(BUILD_SHARED_LIBS ON)
add_subdirectory(lib/karto_sdk)
find_package(catkin REQUIRED COMPONENTS
cmake_modules
message_filters
nav_msgs
karto_sdk
rosconsole
roscpp
sensor_msgs
sparse_bundle_adjustment
tf2
tf2_ros
tf
visualization_msgs
pluginlib
tf2_geometry_msgs
interactive_markers
map_server
slam_toolbox_msgs
)
find_package(PkgConfig REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(CSparse REQUIRED)
find_package(G2O REQUIRED)
find_package(Cholmod REQUIRED)
find_package(LAPACK REQUIRED)
find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
find_package(rostest REQUIRED)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
add_definitions(-DQT_NO_KEYWORDS)
find_package(Boost REQUIRED system serialization filesystem thread)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS
include
${Boost_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${TBB_INCLUDE_DIRS}
LIBRARIES
ceres_solver_plugin
slam_toolbox_rviz
async_slam_toolbox
sync_slam_toolbox
localization_slam_toolbox
lifelong_slam_toolbox
CATKIN_DEPENDS
message_filters
nav_msgs
rosconsole
roscpp
sparse_bundle_adjustment
sensor_msgs
tf2
tf
tf2_ros
visualization_msgs
pluginlib
message_runtime
tf2_geometry_msgs
interactive_markers
slam_toolbox_msgs
karto_sdk
DEPENDS
EIGEN3
Boost
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${CHOLMOD_INCLUDE_DIR}
${Boost_INCLUDE_DIRS}
${TBB_INCLUDE_DIRS}
)
#### Ceres Plugin
add_library(ceres_solver_plugin solvers/ceres_solver.cpp)
target_link_libraries(ceres_solver_plugin ${catkin_LIBRARIES}
${CERES_LIBRARIES}
${Boost_LIBRARIES}
${TBB_LIBRARIES}
)
#### Tool lib for mapping
add_library(toolbox_common src/slam_toolbox_common.cpp src/map_saver.cpp src/loop_closure_assistant.cpp src/laser_utils.cpp src/slam_mapper.cpp)
target_link_libraries(toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#### Mapping executibles
add_library(async_slam_toolbox src/slam_toolbox_async.cpp)
add_dependencies(async_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp )
target_link_libraries(async_slam_toolbox_node async_slam_toolbox)
add_library(sync_slam_toolbox src/slam_toolbox_sync.cpp)
add_dependencies(sync_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(sync_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(sync_slam_toolbox_node src/slam_toolbox_sync_node.cpp )
target_link_libraries(sync_slam_toolbox_node sync_slam_toolbox)
add_library(localization_slam_toolbox src/slam_toolbox_localization.cpp)
add_dependencies(localization_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(localization_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(localization_slam_toolbox_node src/slam_toolbox_localization_node.cpp )
target_link_libraries(localization_slam_toolbox_node localization_slam_toolbox)
add_library(lifelong_slam_toolbox src/experimental/slam_toolbox_lifelong.cpp)
add_dependencies(lifelong_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp )
target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox)
#### Merging maps tool
add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp)
target_link_libraries(merge_maps_kinematic toolbox_common)
#### testing
#if(CATKIN_ENABLE_TESTING)
# include_directories(test)
# catkin_add_gtest(lifelong_metrics_test test/lifelong_metrics_test.cpp)
# target_link_libraries(lifelong_metrics_test lifelong_slam_toolbox)
#endif()
#### Install
install(TARGETS async_slam_toolbox_node sync_slam_toolbox_node localization_slam_toolbox_node lifelong_slam_toolbox_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS toolbox_common
async_slam_toolbox
sync_slam_toolbox
localization_slam_toolbox
lifelong_slam_toolbox
ceres_solver_plugin
merge_maps_kinematic
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES solver_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

79
Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_lifelong.yaml Normal file
View File

@@ -0,0 +1,79 @@
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
mode: mapping
# lifelong params
lifelong_search_use_tree: false
lifelong_minimum_score: 0.1
lifelong_iou_match: 0.85
lifelong_node_removal_score: 0.04
lifelong_overlap_score_scale: 0.06
lifelong_constraint_multiplier: 0.08
lifelong_nearby_penalty: 0.001
lifelong_candidates_scale: 0.03
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
#map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 10.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

66
Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_localization.yaml Normal file
View File

@@ -0,0 +1,66 @@
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
mode: localization #mapping
# if you'd like to start localizing on bringup in a map and pose
map_file_name: testmap
#map_start_pose: [5.0, 1.0, 0.0]
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 3
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
do_loop_closing: true
loop_match_minimum_chain_size: 3
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
loop_search_maximum_distance: 5.0
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

62
Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_offline.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping #localization
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 10.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 14400.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

70
Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_async.yaml Normal file
View File

@@ -0,0 +1,70 @@
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /f_scan
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
# map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 0.5
resolution: 0.05
max_laser_range: 25.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

70
Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_sync.yaml Normal file
View File

@@ -0,0 +1,70 @@
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
#map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

146
Localizations/Packages/slam_toolbox/slam_toolbox/config/slam_toolbox_default.rviz Normal file
View File

@@ -0,0 +1,146 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /InteractiveMarkers1
- /MarkerArray1
Splitter Ratio: 0.5
Tree Height: 1562
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
- Class: slam_toolbox::SlamToolboxPlugin
Name: SlamToolboxPlugin
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 0; 0; 0
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: true
Show Descriptions: true
Show Visual Aids: true
Update Topic: /slam_toolbox/update
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /slam_toolbox/karto_graph_visualization
Name: MarkerArray
Namespaces:
slam_toolbox: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 237; 237; 237
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/XYOrbit
Distance: 220.73793
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -10.1544714
Y: -12.9322882
Z: 0.0100896517
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.729796767
Target Frame: map
Value: XYOrbit (rviz)
Yaw: 2.80506206
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1965
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 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
Selection:
collapsed: false
SlamToolboxPlugin:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1863
X: 1876
Y: 98

70
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp Normal file
View File

@@ -0,0 +1,70 @@
/*
* slam_toolbox
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_
#include "slam_toolbox/slam_toolbox_common.hpp"
namespace slam_toolbox
{
using namespace ::karto;
class LifelongSlamToolbox : public SlamToolbox
{
public:
LifelongSlamToolbox(ros::NodeHandle& nh);
~LifelongSlamToolbox() {};
// computation metrics
double computeObjectiveScore(const double& intersect_over_union, const double& area_overlap, const double& reading_overlap, const int& num_constraints, const double& initial_score, const int& num_candidates) const;
static double computeIntersect(LocalizedRangeScan* s1, LocalizedRangeScan* s2);
static double computeIntersectOverUnion(LocalizedRangeScan* s1, LocalizedRangeScan* s2);
static double computeAreaOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan);
static double computeReadingOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan);
static void computeIntersectBounds(LocalizedRangeScan* s1, LocalizedRangeScan* s2, double& x_l, double& x_u, double& y_l, double& y_u);
protected:
virtual void laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
virtual bool deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;
void evaluateNodeDepreciation(LocalizedRangeScan* range_scan);
void removeFromSlamGraph(Vertex<LocalizedRangeScan>* vertex);
double computeScore(LocalizedRangeScan* reference_scan, Vertex<LocalizedRangeScan>* candidate, const double& initial_score, const int& num_candidates);
ScoredVertices computeScores(Vertices& near_scans, LocalizedRangeScan* range_scan);
Vertices FindScansWithinRadius(LocalizedRangeScan* scan, const double& radius);
void updateScoresSlamGraph(const double& score, Vertex<LocalizedRangeScan>* vertex);
void checkIsNotNormalized(const double& value);
bool use_tree_;
double iou_thresh_;
double removal_score_;
double overlap_scale_;
double constraint_scale_;
double candidates_scale_;
double iou_match_;
double nearby_penalty_;
};
}
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_

71
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp Normal file
View File

@@ -0,0 +1,71 @@
/*
* snap_utils
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_POSE_UTILS_H_
#define SLAM_TOOLBOX_POSE_UTILS_H_
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include "slam_toolbox/toolbox_types.hpp"
#include "karto_sdk/Mapper.h"
namespace pose_utils
{
// helper to get the robots position
class GetPoseHelper
{
public:
GetPoseHelper(tf2_ros::Buffer* tf,
const std::string& base_frame,
const std::string& odom_frame)
: tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame)
{
};
bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
{
geometry_msgs::TransformStamped base_ident, odom_pose;
base_ident.header.stamp = t;
base_ident.header.frame_id = base_frame_;
base_ident.transform.rotation.w = 1.0;
try
{
odom_pose = tf_->transform(base_ident, odom_frame_);
}
catch(tf2::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
const double yaw = tf2::getYaw(odom_pose.transform.rotation);
karto_pose = karto::Pose2(odom_pose.transform.translation.x,
odom_pose.transform.translation.y, yaw);
return true;
};
private:
tf2_ros::Buffer* tf_;
std::string base_frame_, odom_frame_;
};
} // end namespace
#endif //SLAM_TOOLBOX_POSE_UTILS_H_

106
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/laser_utils.hpp Normal file
View File

@@ -0,0 +1,106 @@
/*
* toolbox_types
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_LASER_UTILS_H_
#define SLAM_TOOLBOX_LASER_UTILS_H_
#include <string>
#include "ros/ros.h"
#include "slam_toolbox/toolbox_types.hpp"
#include "tf2/utils.h"
#include "karto_sdk/Mapper.h"
namespace laser_utils
{
// Convert a laser scan to a vector of readings
inline std::vector<double> scanToReadings(const sensor_msgs::LaserScan& scan, const bool& inverted)
{
std::vector<double> readings;
if (inverted)
{
for(std::vector<float>::const_reverse_iterator it = scan.ranges.rbegin(); it != scan.ranges.rend(); ++it)
{
readings.push_back(*it);
}
}
else
{
for(std::vector<float>::const_iterator it = scan.ranges.begin(); it != scan.ranges.end(); ++it)
{
readings.push_back(*it);
}
}
return readings;
};
// Store laser scanner information
class LaserMetadata
{
public:
LaserMetadata();
~LaserMetadata();
LaserMetadata(karto::LaserRangeFinder* lsr, bool invert);
bool isInverted() const;
karto::LaserRangeFinder* getLaser();
void invertScan(sensor_msgs::LaserScan& scan) const;
private:
karto::LaserRangeFinder* laser;
bool inverted;
};
// Help take a scan from a laser and create a laser object
class LaserAssistant
{
public:
LaserAssistant(ros::NodeHandle& nh, tf2_ros::Buffer* tf, const std::string& base_frame);
~LaserAssistant();
LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan);
private:
karto::LaserRangeFinder* makeLaser(const double& mountingYaw);
bool isInverted(double& mountingYaw);
ros::NodeHandle nh_;
tf2_ros::Buffer* tf_;
sensor_msgs::LaserScan scan_;
std::string frame_, base_frame_;
geometry_msgs::TransformStamped laser_pose_;
};
// Hold some scans and utilities around them
class ScanHolder
{
public:
ScanHolder(std::map<std::string, laser_utils::LaserMetadata>& lasers);
~ScanHolder();
sensor_msgs::LaserScan getCorrectedScan(const int& id);
void addScan(const sensor_msgs::LaserScan scan);
private:
std::unique_ptr<std::vector<sensor_msgs::LaserScan> > current_scans_;
std::map<std::string, laser_utils::LaserMetadata>& lasers_;
};
} // end namespace
#endif //SLAM_TOOLBOX_LASER_UTILS_H_

79
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp Normal file
View File

@@ -0,0 +1,79 @@
/*
* loop_closure_assistant
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_
#define SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_
#include <functional>
#include <boost/thread.hpp>
#include <map>
#include "ros/ros.h"
#include "interactive_markers/interactive_marker_server.h"
#include "interactive_markers/menu_handler.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/utils.h"
#include "karto_sdk/Mapper.h"
#include "slam_toolbox/toolbox_types.hpp"
#include "slam_toolbox/laser_utils.hpp"
#include "slam_toolbox/visualization_utils.hpp"
namespace loop_closure_assistant
{
using namespace ::toolbox_types;
class LoopClosureAssistant
{
public:
LoopClosureAssistant(ros::NodeHandle& node, karto::Mapper* mapper, laser_utils::ScanHolder* scan_holder, PausedState& state, ProcessType& processor_type);
void clearMovedNodes();
void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
void publishGraph();
void setMapper(karto::Mapper * mapper);
private:
bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp);
bool clearChangesCallback(slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp);
bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp);
void moveNode(const int& id, const Eigen::Vector3d& pose);
void addMovedNodes(const int& id, Eigen::Vector3d vec);
std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
laser_utils::ScanHolder* scan_holder_;
ros::Publisher scan_publisher_, marker_publisher_;
ros::ServiceServer ssClear_manual_, ssLoopClosure_, ssInteractive_;
boost::mutex moved_nodes_mutex_;
std::map<int, Eigen::Vector3d> moved_nodes_;
karto::Mapper* mapper_;
karto::ScanSolver* solver_;
std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;
visualization_msgs::MarkerArray marker_array_;
boost::mutex interactive_mutex_;
bool interactive_mode_, enable_interactive_mode_;
ros::NodeHandle& nh_;
std::string map_frame_;
PausedState& state_;
ProcessType& processor_type_;
};
} // end namespace
#endif //SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_

50
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/map_saver.hpp Normal file
View File

@@ -0,0 +1,50 @@
/*
* map_saver
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_MAP_SAVER_H_
#define SLAM_TOOLBOX_MAP_SAVER_H_
#include <string>
#include "ros/ros.h"
#include "slam_toolbox/toolbox_msgs.hpp"
namespace map_saver
{
// a service to save a map with a given name as requested
class MapSaver
{
public:
MapSaver(ros::NodeHandle& nh, const std::string& service_name);
protected:
bool saveMapCallback(slam_toolbox_msgs::SaveMap::Request& req,
slam_toolbox_msgs::SaveMap::Response& resp);
void mapCallback(const nav_msgs::OccupancyGrid& msg);
private:
ros::NodeHandle nh_;
ros::ServiceServer server_;
ros::Subscriber sub_;
std::string service_name_, map_name_;
bool received_map_;
};
} // end namespace
#endif //SLAM_TOOLBOX_MAP_SAVER_H_

96
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/merge_maps_kinematic.hpp Normal file
View File

@@ -0,0 +1,96 @@
/*
* Author
* Copyright (c) 2018, Simbe Robotics, Inc.
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_
#define SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_
#include <string>
#include <map>
#include <memory>
#include <vector>
#include <sys/stat.h>
#include <unistd.h>
#include <fstream>
#include <boost/thread.hpp>
#include "ros/ros.h"
#include "interactive_markers/interactive_marker_server.h"
#include "interactive_markers/menu_handler.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/utils.h"
#include "karto_sdk/Mapper.h"
#include "slam_toolbox/toolbox_types.hpp"
#include "slam_toolbox/laser_utils.hpp"
#include "slam_toolbox/visualization_utils.hpp"
using namespace toolbox_types;
class MergeMapsKinematic
{
typedef std::vector<karto::LocalizedRangeScanVector>::iterator LocalizedRangeScansVecIt;
typedef karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt;
public:
MergeMapsKinematic();
~MergeMapsKinematic();
private:
// setup
void setup();
// callback
bool mergeMapCallback(slam_toolbox_msgs::MergeMaps::Request& req, slam_toolbox_msgs::MergeMaps::Response& resp);
bool addSubmapCallback(slam_toolbox_msgs::AddSubmap::Request& req, slam_toolbox_msgs::AddSubmap::Response& resp);
void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
void kartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans, nav_msgs::GetMap::Response& map);
void transformScan(LocalizedRangeScansIt iter, tf2::Transform& submap_correction);
//apply transformation to correct pose
karto::Pose2 applyCorrection(const karto::Pose2& pose, const tf2::Transform& submap_correction);
karto::Vector2<kt_double> applyCorrection(const karto::Vector2<kt_double>& pose, const tf2::Transform& submap_correction);
// ROS-y-ness
ros::NodeHandle nh_;
std::vector<ros::Publisher> sstS_, sstmS_;
ros::ServiceServer ssMap_, ssSubmap_;
//karto bookkeeping
std::map<std::string, laser_utils::LaserMetadata> lasers_;
std::vector<std::unique_ptr<karto::Dataset> > dataset_vec_;
// TF
std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
// visualization
std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;
// state
std::map<int, Eigen::Vector3d> submap_locations_;
std::vector<karto::LocalizedRangeScanVector> scans_vec_;
std::map<int, tf2::Transform> submap_marker_transform_;
double resolution_;
int num_submaps_;
};
#endif //SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_

81
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/serialization.hpp Normal file
View File

@@ -0,0 +1,81 @@
/*
* Author
* Copyright (c) 2018, Simbe Robotics, Inc.
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SERIALIZATION_H_
#define SLAM_TOOLBOX_SERIALIZATION_H_
#include <vector>
#include <string>
#include <ros/ros.h>
#include <karto_sdk/Karto.h>
#include <karto_sdk/Mapper.h>
#include <sys/stat.h>
namespace serialization
{
inline bool fileExists(const std::string& name)
{
struct stat buffer;
return (stat (name.c_str(), &buffer) == 0);
}
inline void write(const std::string& filename,
karto::Mapper& mapper,
karto::Dataset& dataset)
{
try
{
mapper.SaveToFile(filename + std::string(".posegraph"));
dataset.SaveToFile(filename + std::string(".data"));
}
catch (boost::archive::archive_exception e)
{
ROS_ERROR("Failed to write file: Exception %s", e.what());
}
}
inline bool read(const std::string& filename,
karto::Mapper& mapper,
karto::Dataset& dataset)
{
if (!fileExists(filename + std::string(".posegraph")))
{
ROS_ERROR("serialization::Read: Failed to open "
"requested file: %s.", filename.c_str());
return false;
}
try
{
mapper.LoadFromFile(filename + std::string(".posegraph"));
dataset.LoadFromFile(filename + std::string(".data"));
return true;
}
catch (boost::archive::archive_exception e)
{
ROS_ERROR("serialization::Read: Failed to read file: "
"Exception: %s", e.what());
}
return false;
}
} // end namespace
#endif //SLAM_TOOLBOX_SERIALIZATION_H_

67
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_mapper.hpp Normal file
View File

@@ -0,0 +1,67 @@
/*
* Author
* Copyright (c) 2019 Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_MAPPER_H_
#define SLAM_TOOLBOX_SLAM_MAPPER_H_
#include "ros/ros.h"
#include "karto_sdk/Mapper.h"
#include "karto_sdk/Karto.h"
#include "tf2/utils.h"
namespace mapper_utils
{
using namespace ::karto;
class SMapper
{
public:
SMapper();
~SMapper();
// get occupancy grid from scans
karto::OccupancyGrid* getOccupancyGrid(const double& resolution);
// convert Karto pose to TF pose
tf2::Transform toTfPose(const karto::Pose2& pose) const;
// convert TF pose to karto pose
karto::Pose2 toKartoPose(const tf2::Transform& pose) const;
void configure(const ros::NodeHandle& nh);
void Reset();
// // processors
// kt_bool ProcessAtDock(LocalizedRangeScan* pScan);
// kt_bool ProcessAgainstNode(LocalizedRangeScan* pScan, const int& nodeId);
// kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan);
// kt_bool ProcessLocalization(LocalizedRangeScan* pScan);
void setMapper(karto::Mapper* mapper);
karto::Mapper* getMapper();
void clearLocalizationBuffer();
protected:
std::unique_ptr<karto::Mapper> mapper_;
};
} // end namespace
#endif //SLAM_TOOLBOX_SLAM_MAPPER_H_

44
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp Normal file
View File

@@ -0,0 +1,44 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_
#include "slam_toolbox/slam_toolbox_common.hpp"
namespace slam_toolbox
{
class AsynchronousSlamToolbox : public SlamToolbox
{
public:
AsynchronousSlamToolbox(ros::NodeHandle& nh);
~AsynchronousSlamToolbox() {};
protected:
virtual void laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
virtual bool deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;
};
}
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_

156
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp Normal file
View File

@@ -0,0 +1,156 @@
/*
* slam_toolbox
* Copyright (c) 2008, Willow Garage, Inc.
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_
#include "ros/ros.h"
#include "message_filters/subscriber.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "pluginlib/class_loader.h"
#include "slam_toolbox/toolbox_types.hpp"
#include "slam_toolbox/slam_mapper.hpp"
#include "slam_toolbox/snap_utils.hpp"
#include "slam_toolbox/laser_utils.hpp"
#include "slam_toolbox/get_pose_helper.hpp"
#include "slam_toolbox/map_saver.hpp"
#include "slam_toolbox/loop_closure_assistant.hpp"
#include <string>
#include <map>
#include <vector>
#include <queue>
#include <cstdlib>
#include <fstream>
#include <boost/thread.hpp>
#include <sys/resource.h>
namespace slam_toolbox
{
// dirty, dirty cheat I love
using namespace ::toolbox_types;
class SlamToolbox
{
public:
SlamToolbox(ros::NodeHandle& nh);
~SlamToolbox();
bool getInitialized() {return this->initialized_;}
protected:
// threads
void publishVisualizations();
void publishTransformLoop(const double& transform_publish_period);
// setup
void setParams(ros::NodeHandle& nh);
void setSolver(ros::NodeHandle& private_nh_);
void setROSInterfaces(ros::NodeHandle& node);
// callbacks
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) = 0;
bool mapCallback(nav_msgs::GetMap::Request& req,
nav_msgs::GetMap::Response& res);
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request& req,
slam_toolbox_msgs::SerializePoseGraph::Response& resp);
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp);
virtual bool resetCallback(slam_toolbox_msgs::Reset::Request& req,
slam_toolbox_msgs::Reset::Response& resp);
void loadSerializedPoseGraph(std::unique_ptr<karto::Mapper>&, std::unique_ptr<karto::Dataset>&);
void loadPoseGraphByParams(ros::NodeHandle& nh);
// functional bits
karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan);
virtual karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose);
karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose);
bool updateMap();
tf2::Stamped<tf2::Transform> setTransformFromPoses(const karto::Pose2& pose,
const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform);
karto::LocalizedRangeScan* getLocalizedRangeScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose);
bool shouldStartWithPoseGraph(std::string& filename, geometry_msgs::Pose2D& pose, bool& start_at_dock);
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose);
void publishPose(const karto::Pose2 & pose, const karto::Matrix3 & cov, const ros::Time & t);
// pausing bits
bool isPaused(const PausedApplication& app);
bool pauseNewMeasurementsCallback(slam_toolbox_msgs::Pause::Request& req,
slam_toolbox_msgs::Pause::Response& resp);
// ROS-y-ness
ros::NodeHandle nh_;
std::unique_ptr<tf2_ros::Buffer> tf_;
std::unique_ptr<tf2_ros::TransformListener> tfL_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
std::unique_ptr<message_filters::Subscriber<sensor_msgs::LaserScan> > scan_filter_sub_;
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > scan_filter_;
ros::Publisher sst_, sstm_, pose_pub_;
ros::ServiceServer ssMap_, ssPauseMeasurements_, ssSerialize_, ssDesserialize_, ssReset_;
// Storage for ROS parameters
std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_;
int throttle_scans_;
double resolution_;
double position_covariance_scale_;
double yaw_covariance_scale_;
bool first_measurement_, enable_interactive_mode_;
// Book keeping
std::unique_ptr<mapper_utils::SMapper> smapper_;
std::unique_ptr<karto::Dataset> dataset_;
std::map<std::string, laser_utils::LaserMetadata> lasers_;
// helpers
std::unique_ptr<laser_utils::LaserAssistant> laser_assistant_;
std::unique_ptr<pose_utils::GetPoseHelper> pose_helper_;
std::unique_ptr<map_saver::MapSaver> map_saver_;
std::unique_ptr<loop_closure_assistant::LoopClosureAssistant> closure_assistant_;
std::unique_ptr<laser_utils::ScanHolder> scan_holder_;
// Internal state
bool initialized_;
bool using_loop_;
std::vector<std::unique_ptr<boost::thread> > threads_;
tf2::Transform map_to_odom_;
boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_;
PausedState state_;
nav_msgs::GetMap::Response map_;
ProcessType processor_type_;
std::unique_ptr<karto::Pose2> process_near_pose_;
tf2::Transform reprocessing_transform_;
// pluginlib
pluginlib::ClassLoader<karto::ScanSolver> solver_loader_;
boost::shared_ptr<karto::ScanSolver> solver_;
};
} // end namespace
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_

62
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp Normal file
View File

@@ -0,0 +1,62 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_
#include "slam_toolbox/slam_toolbox_common.hpp"
#include "std_srvs/Empty.h"
namespace slam_toolbox
{
using namespace ::karto;
class LocalizationSlamToolbox : public SlamToolbox
{
public:
LocalizationSlamToolbox(ros::NodeHandle& nh);
~LocalizationSlamToolbox() {};
protected:
virtual void laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
void localizePoseCallback(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
bool clearLocalizationBuffer(
std_srvs::Empty::Request& req,
std_srvs::Empty::Response& resp);
virtual bool serializePoseGraphCallback(
slam_toolbox_msgs::SerializePoseGraph::Request& req,
slam_toolbox_msgs::SerializePoseGraph::Response& resp) override final;
virtual bool deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;
virtual LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose) override final;
ros::Subscriber localization_pose_sub_;
ros::ServiceServer clear_localization_;
};
}
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_

50
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp Normal file
View File

@@ -0,0 +1,50 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_H_
#include "slam_toolbox/slam_toolbox_common.hpp"
namespace slam_toolbox
{
class SynchronousSlamToolbox : public SlamToolbox
{
public:
SynchronousSlamToolbox(ros::NodeHandle& nh);
~SynchronousSlamToolbox() {};
void run();
protected:
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) override final;
bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request& req, slam_toolbox_msgs::ClearQueue::Response& resp);
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;
virtual bool resetCallback(slam_toolbox_msgs::Reset::Request& req,
slam_toolbox_msgs::Reset::Response& resp) override final;
std::queue<PosedScan> q_;
ros::ServiceServer ssClear_;
boost::mutex q_mutex_;
};
}
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_NODE_H_

45
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/snap_utils.hpp Normal file
View File

@@ -0,0 +1,45 @@
/*
* snap_utils
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SNAP_UTILS_H_
#define SLAM_TOOLBOX_SNAP_UTILS_H_
namespace snap_utils
{
// whether this is running in a snap container
inline bool isInSnap()
{
char* snap_common = getenv("SNAP_COMMON");
if (snap_common != NULL)
{
return true;
}
return false;
};
// get path of shared space
inline std::string getSnapPath()
{
char* snap_common = getenv("SNAP_COMMON");
return std::string(snap_common);
}
} // end namespace
#endif //SLAM_TOOLBOX_SNAP_UTILS_H_

45
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_msgs.hpp Normal file
View File

@@ -0,0 +1,45 @@
/*
* slam_toolbox
* Copyright (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_TOOLBOX_MSGS_H_
#define SLAM_TOOLBOX_TOOLBOX_MSGS_H_
#include <nav_msgs/MapMetaData.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/GetMap.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/InteractiveMarker.h>
#include <visualization_msgs/InteractiveMarkerControl.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include "slam_toolbox_msgs/Pause.h"
#include "slam_toolbox_msgs/ClearQueue.h"
#include "slam_toolbox_msgs/ToggleInteractive.h"
#include "slam_toolbox_msgs/Clear.h"
#include "slam_toolbox_msgs/ClearQueue.h"
#include "slam_toolbox_msgs/SaveMap.h"
#include "slam_toolbox_msgs/LoopClosure.h"
#include "slam_toolbox_msgs/SerializePoseGraph.h"
#include "slam_toolbox_msgs/DeserializePoseGraph.h"
#include "slam_toolbox_msgs/MergeMaps.h"
#include "slam_toolbox_msgs/AddSubmap.h"
#include "slam_toolbox_msgs/Reset.h"
#endif //SLAM_TOOLBOX_TOOLBOX_MSGS_H_

129
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_types.hpp Normal file
View File

@@ -0,0 +1,129 @@
/*
* toolbox_types
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_TOOLBOX_TYPES_H_
#define SLAM_TOOLBOX_TOOLBOX_TYPES_H_
#include <map>
#include <vector>
#include "tf/transform_datatypes.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "slam_toolbox/toolbox_msgs.hpp"
#include "karto_sdk/Mapper.h"
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
namespace toolbox_types
{
// object containing a scan pointer and a position
struct PosedScan
{
PosedScan(sensor_msgs::LaserScan::ConstPtr scan_in, karto::Pose2 pose_in) :
scan(scan_in), pose(pose_in)
{
}
sensor_msgs::LaserScan::ConstPtr scan;
karto::Pose2 pose;
};
// object containing a vertex pointer and an updated score
struct ScoredVertex
{
ScoredVertex(karto::Vertex<karto::LocalizedRangeScan>* vertex, double score)
: vertex_(vertex), score_(score)
{
}
double GetScore()
{
return score_;
}
karto::Vertex<karto::LocalizedRangeScan>* GetVertex()
{
return vertex_;
}
karto::Vertex<karto::LocalizedRangeScan>* vertex_;
double score_;
};
typedef std::vector<ScoredVertex> ScoredVertices;
typedef std::vector<karto::Vertex<karto::LocalizedRangeScan>*> Vertices;
// types of pause functionality available
enum PausedApplication
{
PROCESSING = 0,
VISUALIZING_GRAPH = 1,
NEW_MEASUREMENTS = 2
};
// types of sensor processing
enum ProcessType
{
PROCESS = 0,
PROCESS_FIRST_NODE = 1,
PROCESS_NEAR_REGION = 2,
PROCESS_LOCALIZATION = 3
};
// Pause state
struct PausedState
{
PausedState()
{
state_map_[NEW_MEASUREMENTS] = false;
state_map_[VISUALIZING_GRAPH] = false;
state_map_[PROCESSING] = false;
};
void set(const PausedApplication& app, const bool& state)
{
boost::mutex::scoped_lock lock(pause_mutex_);
state_map_[app] = state;
};
bool get(const PausedApplication& app)
{
boost::mutex::scoped_lock lock(pause_mutex_);
return state_map_[app];
};
std::map<PausedApplication, bool> state_map_;
boost::mutex pause_mutex_;
};
typedef std::map<karto::Name, std::map<int, karto::Vertex<karto::LocalizedRangeScan>*>> VerticeMap;
typedef std::vector<karto::Edge<karto::LocalizedRangeScan>*> EdgeVector;
typedef std::map<int, karto::Vertex<karto::LocalizedRangeScan>*> ScanMap;
typedef std::vector<karto::Vertex<karto::LocalizedRangeScan>*> ScanVector;
typedef slam_toolbox_msgs::DeserializePoseGraph::Request procType;
typedef std::unordered_map<int, Eigen::Vector3d>::iterator GraphIterator;
typedef std::unordered_map<int, Eigen::Vector3d>::const_iterator ConstGraphIterator;
} // end namespace
#endif //SLAM_TOOLBOX_TOOLBOX_TYPES_H_

171
Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/visualization_utils.hpp Normal file
View File

@@ -0,0 +1,171 @@
/*
* visualization_utils
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
#define SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
namespace vis_utils
{
inline visualization_msgs::Marker toVertexMarker(
const std::string& frame,
const std::string& ns,
const double& scale)
{
visualization_msgs::Marker marker;
marker.header.frame_id = frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.;
marker.scale.x = scale;
marker.scale.y = scale;
marker.scale.z = scale;
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0.0;
marker.color.a = 1.;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(0.);
return marker;
}
inline visualization_msgs::Marker toEdgeMarker(
const std::string& frame,
const std::string& ns,
const double& width)
{
visualization_msgs::Marker marker;
marker.header.frame_id = frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.;
marker.points.resize(2);
marker.scale.x = width;
marker.scale.y = 0;
marker.scale.z = 0;
marker.color.r = 0.0;
marker.color.g = 0;
marker.color.b = 1.0;
marker.color.a = 1.;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(0.);
return marker;
}
inline visualization_msgs::InteractiveMarker toInteractiveMarker(
visualization_msgs::Marker& marker,
const double& scale)
{
// marker basics
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = marker.header.frame_id;
int_marker.header.stamp = ros::Time::now();
int_marker.name = std::to_string(marker.id);
int_marker.pose.orientation.w = 1.;
int_marker.pose.position.x = marker.pose.position.x;
int_marker.pose.position.y = marker.pose.position.y;
int_marker.scale = scale;
// translate control
visualization_msgs::InteractiveMarkerControl control;
control.orientation_mode =
visualization_msgs::InteractiveMarkerControl::FIXED;
control.always_visible = true;
control.orientation.w = 0;
control.orientation.x = 0.7071;
control.orientation.y = 0;
control.orientation.z = 0.7071;
control.interaction_mode =
visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
control.markers.push_back( marker );
int_marker.controls.push_back( control );
// rotate control
visualization_msgs::InteractiveMarkerControl control_rot;
control_rot.orientation_mode =
visualization_msgs::InteractiveMarkerControl::FIXED;
control_rot.always_visible = true;
control_rot.orientation.w = 0;
control_rot.orientation.x = 0.7071;
control_rot.orientation.y = 0;
control_rot.orientation.z = 0.7071;
control_rot.interaction_mode =
visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back( control_rot );
return int_marker;
}
inline void toNavMap(
const karto::OccupancyGrid* occ_grid,
nav_msgs::OccupancyGrid& map)
{
// Translate to ROS format
kt_int32s width = occ_grid->GetWidth();
kt_int32s height = occ_grid->GetHeight();
karto::Vector2<kt_double> offset =
occ_grid->GetCoordinateConverter()->GetOffset();
if(map.info.width != (unsigned int) width ||
map.info.height != (unsigned int) height ||
map.info.origin.position.x != offset.GetX() ||
map.info.origin.position.y != offset.GetY())
{
map.info.origin.position.x = offset.GetX();
map.info.origin.position.y = offset.GetY();
map.info.width = width;
map.info.height = height;
map.data.resize(map.info.width * map.info.height);
}
for (kt_int32s y = 0; y < height; y++)
{
for (kt_int32s x = 0; x < width; x++)
{
kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
switch (value)
{
case karto::GridStates_Unknown:
map.data[MAP_IDX(map.info.width, x, y)] = -1;
break;
case karto::GridStates_Occupied:
map.data[MAP_IDX(map.info.width, x, y)] = 100;
break;
case karto::GridStates_Free:
map.data[MAP_IDX(map.info.width, x, y)] = 0;
break;
default:
ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
break;
}
}
}
return;
}
} // end namespace
#endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_

7
Localizations/Packages/slam_toolbox/slam_toolbox/launch/lifelong.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="slam_toolbox" type="lifelong_slam_toolbox_node" name="slam_toolbox" output="screen">
<rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_lifelong.yaml" />
</node>
</launch>

11
Localizations/Packages/slam_toolbox/slam_toolbox/launch/localization.launch Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<param name="/use_sim_time" value="true" />
<node pkg="tf" type="static_transform_publisher" name="base_to_scan_broadcaster" args="0.0 0 0 0 0 0 base_footprint base_scan 100" />
<node pkg="slam_toolbox" type="localization_slam_toolbox_node" name="slam_toolbox" output="screen">
<rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_localization.yaml" />
</node>
</launch>

6
Localizations/Packages/slam_toolbox/slam_toolbox/launch/merge_maps_kinematic.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node pkg="slam_toolbox" type="merge_maps_kinematic" name="merge_maps_kinematic" output="screen">
</node>
</launch>

7
Localizations/Packages/slam_toolbox/slam_toolbox/launch/offline.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="slam_toolbox" type="sync_slam_toolbox_node" name="slam_toolbox" output="screen">
<rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_offline.yaml" />
</node>
</launch>

11
Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_async.launch Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<param name="/use_sim_time" value="false" />
<!-- <node pkg="tf" type="static_transform_publisher" name="base_to_scan_broadcaster" args="0 0 0 0 0 0 base_footprint base_scan 100" /> -->
<node pkg="slam_toolbox" type="async_slam_toolbox_node" name="slam_toolbox" output="screen">
<rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_online_async.yaml" />
</node>
</launch>

7
Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_sync.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="slam_toolbox" type="sync_slam_toolbox_node" name="slam_toolbox" output="screen">
<rosparam command="load" file="$(find slam_toolbox)/config/mapper_params_online_sync.yaml" />
</node>
</launch>

7
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/Authors Normal file
View File

@@ -0,0 +1,7 @@
Karto Open Source Library 1.0
Contributors:
-------------------------------
Michael A. Eriksen (eriksen@ai.sri.com)
Benson Limketkai (bensonl@ai.sri.com)
Steven Macenski (steven.macenski@simberobotics.com)

37
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,37 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package karto_sdk
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.1.4 (2016-03-02)
------------------
* update build status badges
* Adds LocalizedRangeScanWithPoints range scan
* Contributors: Michael Ferguson, Russell Toris
1.1.3 (2016-02-16)
------------------
* Link against, and export depend on, boost
* Contributors: Hai Nguyen, Michael Ferguson
1.1.2 (2015-07-13)
------------------
* Added getters and setters for parameters inside Mapper so they can be changed via the ROS param server.
* Contributors: Luc Bettaieb, Michael Ferguson
1.1.1 (2015-05-07)
------------------
* Makes FindValidPoints robust to the first point in the scan being a NaN
* Bump minimum cmake version requirement
* Fix cppcheck warnings
* Fix newlines (dos2unix) & superfluous whitespace
* Protect functions that throw away const-ness (check dirty flag) with mutex
* Don't modify scan during loop closure check - work on a copy of it
* removed useless return to avoid cppcheck error
* Add Mapper::SetUseScanMatching
* Remove html entities from log output
* Fix NANs cause raytracing to take forever
* Contributors: Daniel Pinyol, Michael Ferguson, Paul Mathieu, Siegfried-A. Gevatter Pujals, liz-murphy
1.1.0 (2014-06-15)
------------------
* Release as a pure catkin ROS package

34
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CMakeLists.txt Normal file
View File

@@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 2.8.3)
project(karto_sdk)
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0")
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
find_package(catkin REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system serialization filesystem thread)
find_package(TBB REQUIRED)
add_compile_options(-std=c++17)
catkin_package(
DEPENDS
Boost
TBB
INCLUDE_DIRS
include
${TBB_INCLUDE_DIRS}
LIBRARIES
kartoSlamToolbox
)
add_definitions(${EIGEN3_DEFINITIONS})
include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp)
target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})
install(DIRECTORY include/ DESTINATION include)
install(TARGETS kartoSlamToolbox
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

165
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/LICENSE Normal file
View File

@@ -0,0 +1,165 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
You may place library facilities that are a work based on the
Library side by side in a single library together with other library
facilities that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU Lesser General Public License from time to time. Such new
versions will be similar in spirit to the present version, but may
differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the
Library as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.

292
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/cmake/FindTBB.cmake Normal file
View File

@@ -0,0 +1,292 @@
# - Find ThreadingBuildingBlocks include dirs and libraries
# Use this module by invoking find_package with the form:
# find_package(TBB
# [REQUIRED] # Fail with error if TBB is not found
# ) #
# Once done, this will define
#
# TBB_FOUND - system has TBB
# TBB_INCLUDE_DIRS - the TBB include directories
# TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or
# malloc proxy
#
# TBB_VERSION_MAJOR - Major Product Version Number
# TBB_VERSION_MINOR - Minor Product Version Number
# TBB_INTERFACE_VERSION - Engineering Focused Version Number
# TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version
# still supported. This uses the engineering
# focused interface version numbers.
#
# TBB_MALLOC_FOUND - system has TBB malloc library
# TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories
# TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined
#
# TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library
# TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories
# TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined
#
#
# This module reads hints about search locations from variables:
# ENV TBB_ARCH_PLATFORM for windows only
# ENV TBB_ROOT or just TBB_ROOT
#
#
#
# Modified by Robert Maynard from the original OGRE source
#
#-------------------------------------------------------------------
# This file is part of the CMake build system for OGRE
# (Object-oriented Graphics Rendering Engine)
# For the latest info, see http://www.ogre3d.org/
#
# The contents of this file are placed in the public domain. Feel
# free to make use of it in any way you like.
#-------------------------------------------------------------------
#
#=============================================================================
# Copyright 2010-2012 Kitware, Inc.
# Copyright 2012 Rolf Eike Beer <eike@sf-mail.de>
#
# Distributed under the OSI-approved BSD License (the "License");
# see accompanying file Copyright.txt for details.
#
# This software is distributed WITHOUT ANY WARRANTY; without even the
# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the License for more information.
#=============================================================================
# (To distribute this file outside of CMake, substitute the full
# License text for the above reference.)
#=============================================================================
# FindTBB helper functions and macros
#
#===============================================
# Create search paths based on prefix path
#===============================================
macro(create_search_paths PREFIX)
foreach(dir ${${PREFIX}_PREFIX_PATH})
set(${PREFIX}_INC_SEARCH_PATH ${${PREFIX}_INC_SEARCH_PATH}
${dir}/include ${dir}/Include ${dir}/include/${PREFIX})
set(${PREFIX}_LIB_SEARCH_PATH ${${PREFIX}_LIB_SEARCH_PATH}
${dir}/lib ${dir}/Lib ${dir}/lib/${PREFIX} ${dir}/Libs)
endforeach(dir)
endmacro(create_search_paths)
#===============================================
# Do the final processing for the package find.
#===============================================
macro(findpkg_finish PREFIX)
# skip if already processed during this run
if (NOT ${PREFIX}_FOUND)
if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY)
set(${PREFIX}_FOUND TRUE)
set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR})
set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY})
else ()
if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY)
message(FATAL_ERROR "Required library ${PREFIX} not found.")
endif ()
endif ()
#mark the following variables as internal variables
mark_as_advanced(${PREFIX}_INCLUDE_DIR
${PREFIX}_LIBRARY
${PREFIX}_LIBRARY_DEBUG
${PREFIX}_LIBRARY_RELEASE)
endif ()
endmacro(findpkg_finish)
#===============================================
# Generate debug names from given RELEASEease names
#===============================================
macro(get_debug_names PREFIX)
foreach(i ${${PREFIX}})
set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i})
endforeach(i)
endmacro(get_debug_names)
#===============================================
# See if we have env vars to help us find tbb
#===============================================
macro(getenv_path VAR)
set(ENV_${VAR} $ENV{${VAR}})
# replace won't work if var is blank
if (ENV_${VAR})
string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} )
endif ()
endmacro(getenv_path)
#===============================================
# Couple a set of RELEASEease AND debug libraries
#===============================================
macro(make_library_set PREFIX)
if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG)
set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG})
elseif (${PREFIX}_RELEASE)
set(${PREFIX} ${${PREFIX}_RELEASE})
elseif (${PREFIX}_DEBUG)
set(${PREFIX} ${${PREFIX}_DEBUG})
endif ()
endmacro(make_library_set)
#=============================================================================
# Now to actually find TBB
#
# Get path, convert backslashes as ${ENV_${var}}
getenv_path(TBB_ROOT)
# construct search paths
set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT})
create_search_paths(TBB)
# get the arch, only used by windows
if($ENV{TBB_ARCH_PLATFORM})
set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM})
endif()
# For Windows, let's assume that the user might be using the precompiled
# TBB packages from the main website. These use a rather awkward directory
# structure (at least for automatically finding the right files) depending
# on platform and compiler, but we'll do our best to accommodate it.
# Not adding the same effort for the precompiled linux builds, though. Those
# have different versions for CC compiler versions and linux kernels which
# will never adequately match the user's setup, so there is no feasible way
# to detect the "best" version to use. The user will have to manually
# select the right files. (Chances are the distributions are shipping their
# custom version of tbb, anyway, so the problem is probably nonexistant.)
if (WIN32 AND MSVC)
set(COMPILER_PREFIX "vc7.1")
if (MSVC_VERSION EQUAL 1400)
set(COMPILER_PREFIX "vc8")
elseif(MSVC_VERSION EQUAL 1500)
set(COMPILER_PREFIX "vc9")
elseif(MSVC_VERSION EQUAL 1600)
set(COMPILER_PREFIX "vc10")
elseif(MSVC_VERSION EQUAL 1700)
set(COMPILER_PREFIX "vc11")
elseif(MSVC_VERSION EQUAL 1800)
set(COMPILER_PREFIX "vc12")
endif ()
# for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path
foreach (dir ${TBB_PREFIX_PATH})
if (CMAKE_CL_64)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX})
else ()
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX})
endif ()
endforeach ()
endif ()
foreach (dir ${TBB_PREFIX_PATH})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib)
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM})
list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib)
endforeach ()
set(TBB_LIBRARY_NAMES tbb)
get_debug_names(TBB_LIBRARY_NAMES)
find_path(TBB_INCLUDE_DIR
NAMES tbb/tbb.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_LIBRARY_RELEASE
NAMES ${TBB_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_LIBRARY_DEBUG
NAMES ${TBB_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_LIBRARY)
findpkg_finish(TBB)
#on unix we need to also link to rt
if(UNIX AND NOT APPLE)
list(APPEND TBB_LIBRARIES rt)
endif()
#if we haven't found TBB no point on going any further
if (NOT TBB_FOUND)
return()
endif ()
#=============================================================================
# Look for TBB's malloc package
set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc)
get_debug_names(TBB_MALLOC_LIBRARY_NAMES)
find_path(TBB_MALLOC_INCLUDE_DIR
NAMES tbb/tbb.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_MALLOC_LIBRARY_RELEASE
NAMES ${TBB_MALLOC_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_MALLOC_LIBRARY_DEBUG
NAMES ${TBB_MALLOC_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_MALLOC_LIBRARY)
findpkg_finish(TBB_MALLOC)
#=============================================================================
# Look for TBB's malloc proxy package
set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy)
get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES)
find_path(TBB_MALLOC_PROXY_INCLUDE_DIR
NAMES tbb/tbbmalloc_proxy.h
PATHS ${TBB_INC_SEARCH_PATH})
find_library(TBB_MALLOC_PROXY_LIBRARY_RELEASE
NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES}
PATHS ${TBB_LIB_SEARCH_PATH})
find_library(TBB_MALLOC_PROXY_LIBRARY_DEBUG
NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES_DEBUG}
PATHS ${TBB_LIB_SEARCH_PATH})
make_library_set(TBB_MALLOC_PROXY_LIBRARY)
findpkg_finish(TBB_MALLOC_PROXY)
#-----------------------------------------------------------------------------
# setup timing libs we need to link too
#=============================================================================
#parse all the version numbers from tbb
if(NOT TBB_VERSION)
#only read the start of the file
file(READ
"${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h"
TBB_VERSION_CONTENTS
LIMIT 2048)
string(REGEX REPLACE
".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
string(REGEX REPLACE
".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}")
endif()

7199
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h Normal file
View File

File diff suppressed because it is too large Load Diff

127
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Macros.h Normal file
View File

@@ -0,0 +1,127 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef karto_sdk_MACROS_H
#define karto_sdk_MACROS_H
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/**
* Karto defines for handling deprecated code
*/
#ifndef KARTO_DEPRECATED
# if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__>=1))
# define KARTO_DEPRECATED __attribute__((deprecated))
# elif defined(__INTEL) || defined(_MSC_VER)
# define KARTO_DEPRECATED __declspec(deprecated)
# else
# define KARTO_DEPRECATED
# endif
#endif
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/**
* Karto defines for windows dynamic build
*/
#if defined(_MSC_VER) || defined(__CYGWIN__) || defined(__MINGW32__) || defined( __BCPLUSPLUS__) || defined( __MWERKS__)
# if defined( _LIB ) || defined( KARTO_STATIC ) || defined( STATIC_BUILD )
# define KARTO_EXPORT
# else
# ifdef KARTO_DYNAMIC
# define KARTO_EXPORT __declspec(dllexport)
# else
# define KARTO_EXPORT __declspec(dllimport)
# endif // KARTO_DYNAMIC
# endif
#else
# define KARTO_EXPORT
#endif
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/**
* Helper defines for std iterator loops
*/
#define forEach( listtype, list ) \
for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter )
#define forEachAs( listtype, list, iter ) \
for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter )
#define const_forEach( listtype, list ) \
for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter )
#define const_forEachAs( listtype, list, iter ) \
for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter )
#define forEachR( listtype, list ) \
for ( listtype::reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter )
#define const_forEachR( listtype, list ) \
for ( listtype::const_reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter )
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/**
* Disable annoying compiler warnings
*/
#if defined(__INTEL) || defined(_MSC_VER)
// Disable the warning: 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2'
#pragma warning(disable:4251)
#endif
#ifdef __INTEL_COMPILER
// Disable the warning: conditional expression is constant
#pragma warning(disable:4127)
// Disable the warning: 'identifier' : unreferenced formal parameter
#pragma warning(disable:4100)
// remark #383: value copied to temporary, reference to temporary used
#pragma warning(disable:383)
// remark #981: operands are evaluated in unspecified order
// disabled -> completely pointless if the functions do not have side effects
#pragma warning(disable:981)
// remark #1418: external function definition with no prior declaration
#pragma warning(disable:1418)
// remark #1572: floating-point equality and inequality comparisons are unreliable
// disabled -> everyone knows it, the parser passes this problem deliberately to the user
#pragma warning(disable:1572)
// remark #10121:
#pragma warning(disable:10121)
#endif // __INTEL_COMPILER
#endif // karto_sdk_MACROS_H

2470
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h Normal file
View File

File diff suppressed because it is too large Load Diff

252
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Math.h Normal file
View File

@@ -0,0 +1,252 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef karto_sdk_MATH_H
#define karto_sdk_MATH_H
#include <assert.h>
#include <math.h>
#include <limits>
#include <karto_sdk/Types.h>
namespace karto
{
/**
* Platform independent pi definitions
*/
const kt_double KT_PI = 3.14159265358979323846; // The value of PI
const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI
const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2
const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180
const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI
/**
* Lets define a small number!
*/
const kt_double KT_TOLERANCE = 1e-06;
/**
* Lets define max value of kt_int32s (int32_t) to use it to mark invalid scans
*/
const kt_int32s INVALID_SCAN = std::numeric_limits<kt_int32s>::max();
namespace math
{
/**
* Converts degrees into radians
* @param degrees
* @return radian equivalent of degrees
*/
inline kt_double DegreesToRadians(kt_double degrees)
{
return degrees * KT_PI_180;
}
/**
* Converts radians into degrees
* @param radians
* @return degree equivalent of radians
*/
inline kt_double RadiansToDegrees(kt_double radians)
{
return radians * KT_180_PI;
}
/**
* Square function
* @param value
* @return square of value
*/
template<typename T>
inline T Square(T value)
{
return (value * value);
}
/**
* Round function
* @param value
* @return rounds value to the nearest whole number (as double)
*/
inline kt_double Round(kt_double value)
{
return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
}
/**
* Binary minimum function
* @param value1
* @param value2
* @return the lesser of value1 and value2
*/
template<typename T>
inline const T& Minimum(const T& value1, const T& value2)
{
return value1 < value2 ? value1 : value2;
}
/**
* Binary maximum function
* @param value1
* @param value2
* @return the greater of value1 and value2
*/
template<typename T>
inline const T& Maximum(const T& value1, const T& value2)
{
return value1 > value2 ? value1 : value2;
}
/**
* Clips a number to the specified minimum and maximum values.
* @param n number to be clipped
* @param minValue minimum value
* @param maxValue maximum value
* @return the clipped value
*/
template<typename T>
inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
{
return Minimum(Maximum(n, minValue), maxValue);
}
/**
* Checks whether two numbers are equal within a certain tolerance.
* @param a
* @param b
* @return true if a and b differ by at most a certain tolerance.
*/
inline kt_bool DoubleEqual(kt_double a, kt_double b)
{
double delta = a - b;
return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
}
/**
* Checks whether value is in the range [0;maximum)
* @param value
* @param maximum
*/
template<typename T>
inline kt_bool IsUpTo(const T& value, const T& maximum)
{
return (value >= 0 && value < maximum);
}
/**
* Checks whether value is in the range [0;maximum)
* Specialized version for unsigned int (kt_int32u)
* @param value
* @param maximum
*/
template<>
inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
{
return (value < maximum);
}
/**
* Checks whether value is in the range [a;b]
* @param value
* @param a
* @param b
*/
template<typename T>
inline kt_bool InRange(const T& value, const T& a, const T& b)
{
return (value >= a && value <= b);
}
/**
* Normalizes angle to be in the range of [-pi, pi]
* @param angle to be normalized
* @return normalized angle
*/
inline kt_double NormalizeAngle(kt_double angle)
{
while (angle < -KT_PI)
{
if (angle < -KT_2PI)
{
angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
}
else
{
angle += KT_2PI;
}
}
while (angle > KT_PI)
{
if (angle > KT_2PI)
{
angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
}
else
{
angle -= KT_2PI;
}
}
assert(math::InRange(angle, -KT_PI, KT_PI));
return angle;
}
/**
* Returns an equivalent angle to the first parameter such that the difference
* when the second parameter is subtracted from this new value is an angle
* in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi.
* @param minuend
* @param subtrahend
* @return normalized angle
*/
inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
{
while (minuend - subtrahend < -KT_PI)
{
minuend += KT_2PI;
}
while (minuend - subtrahend > KT_PI)
{
minuend -= KT_2PI;
}
return minuend;
}
/**
* Align a value to the alignValue.
* The alignValue should be the power of two (2, 4, 8, 16, 32 and so on)
* @param value
* @param alignValue
* @return aligned value
*/
template<class T>
inline T AlignValue(size_t value, size_t alignValue = 8)
{
return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
}
} // namespace math
} // namespace karto
#endif // karto_sdk_MATH_H

69
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Types.h Normal file
View File

@@ -0,0 +1,69 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef karto_sdk_TYPES_H
#define karto_sdk_TYPES_H
#include <cstddef>
/**
* Karto type definition
* Ensures platform independent types for windows, linux and mac
*/
#if defined(_MSC_VER)
typedef signed __int8 kt_int8s;
typedef unsigned __int8 kt_int8u;
typedef signed __int16 kt_int16s;
typedef unsigned __int16 kt_int16u;
typedef signed __int32 kt_int32s;
typedef unsigned __int32 kt_int32u;
typedef signed __int64 kt_int64s;
typedef unsigned __int64 kt_int64u;
#else
#include <stdint.h>
typedef int8_t kt_int8s;
typedef uint8_t kt_int8u;
typedef int16_t kt_int16s;
typedef uint16_t kt_int16u;
typedef int32_t kt_int32s;
typedef uint32_t kt_int32u;
#if defined(__LP64__)
typedef signed long kt_int64s;
typedef unsigned long kt_int64u;
#else
typedef signed long long kt_int64s;
typedef unsigned long long kt_int64u;
#endif
#endif
typedef bool kt_bool;
typedef char kt_char;
typedef float kt_float;
typedef double kt_double;
#endif // karto_sdk_TYPES_H

2039
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

75
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann_adaptors.h Normal file
View File

@@ -0,0 +1,75 @@
/*
* nanoflann_adaptors.h
* Copyright (c) 2018, Simbe Robotics
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "nanoflann.hpp"
// And this is the "dataset to kd-tree" adaptor class:
template <typename Derived>
struct VertexVectorPoseNanoFlannAdaptor
{
const Derived &obj; //!< A const ref to the data set origin
/// The constructor that sets the data set source
VertexVectorPoseNanoFlannAdaptor(const Derived &obj_) : obj(obj_) { }
/// CRTP helper method
inline const Derived& derived() const { return obj; }
// Must return the number of data points
inline size_t kdtree_get_point_count() const { return derived().size(); }
// Returns the dim'th component of the idx'th point in the class:
// Since this is inlined and the "dim" argument is typically an immediate value, the
// "if/else's" are actually solved at compile time.
inline double kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim == 0) return derived()[idx]->GetObject()->GetCorrectedPose().GetX();
else return derived()[idx]->GetObject()->GetCorrectedPose().GetY();
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
}; // end of VertexVectorPoseNanoFlannAdaptor
// And this is the "dataset to kd-tree" adaptor class:
template <typename Derived>
struct VertexVectorScanCenterNanoFlannAdaptor
{
const Derived &obj;
VertexVectorScanCenterNanoFlannAdaptor(const Derived &obj_) : obj(obj_) { }
inline const Derived& derived() const { return obj; }
inline size_t kdtree_get_point_count() const { return derived().size(); }
inline double kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim == 0) return derived()[idx]->GetObject()->GetBarycenterPose().GetX();
else return derived()[idx]->GetObject()->GetBarycenterPose().GetY();
}
template <class BBOX>
bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
}; // end of VertexVectorScanCenterNanoFlannAdaptor

23
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/package.xml Normal file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package>
<name>karto_sdk</name>
<version>1.5.2</version>
<description>Catkinized ROS packaging of the OpenKarto library</description>
<maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
<maintainer email="lbettaieb@fetchrobotics.com">Luc Bettaieb</maintainer>
<license>LGPLv3</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<build_depend>tbb</build_depend>
<build_depend>libblas-dev</build_depend>
<build_depend>liblapack-dev</build_depend>
<run_depend>boost</run_depend>
<run_depend>tbb</run_depend>
<run_depend>libblas-dev</run_depend>
<run_depend>liblapack-dev</run_depend>
</package>

279
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Karto.cpp Normal file
View File

@@ -0,0 +1,279 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "karto_sdk/Karto.h"
#include <boost/serialization/export.hpp>
BOOST_CLASS_EXPORT_IMPLEMENT(karto::NonCopyable);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Object);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Sensor);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorData);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Name);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeScan);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::LocalizedRangeScan);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::CustomData);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Module);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeFinder);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Dataset);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::LookupArray);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorManager);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::AbstractParameter);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterEnum);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameters);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterManager);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<kt_double >);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<karto::Pose2>);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<kt_bool>);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<kt_int32u>);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<kt_int32s>);
BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter<std::string>);
namespace karto
{
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
SensorManager* SensorManager::GetInstance()
{
static Singleton<SensorManager> sInstance;
return sInstance.Get();
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
Object::Object()
: m_pParameterManager(new ParameterManager())
{
}
Object::Object(const Name& rName)
: m_Name(rName)
, m_pParameterManager(new ParameterManager())
{
}
Object::~Object()
{
delete m_pParameterManager;
m_pParameterManager = NULL;
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
Module::Module(const std::string& rName)
: Object(rName)
{
}
Module::~Module()
{
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
Sensor::Sensor(const Name& rName)
: Object(rName)
{
m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
}
Sensor::~Sensor()
{
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
SensorData::SensorData(const Name& rSensorName)
: Object()
, m_StateId(-1)
, m_UniqueId(-1)
, m_SensorName(rSensorName)
, m_Time(0.0)
{
}
SensorData::~SensorData()
{
forEach(CustomDataVector, &m_CustomData)
{
delete *iter;
}
m_CustomData.clear();
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
void CellUpdater::operator() (kt_int32u index)
{
kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
std::ostream& operator << (std::ostream& rStream, Exception& rException)
{
rStream << "Error detect: " << std::endl;
rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;
return rStream;
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
CoordinateConverter* pCoordinateConverter,
kt_bool ignoreThresholdPoints,
kt_bool flipY) const
{
PointVectorDouble pointReadings;
Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();
// compute point readings
kt_int32u beamNum = 0;
kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
{
kt_double rangeReading = pRangeReadings[i];
if (ignoreThresholdPoints)
{
if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
{
continue;
}
}
else
{
rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
}
kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();
Vector2<kt_double> point;
point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
if (pCoordinateConverter != NULL)
{
Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
point.SetX(gridPoint.GetX());
point.SetY(gridPoint.GetY());
}
pointReadings.push_back(point);
}
return pointReadings;
}
kt_bool LaserRangeFinder::Validate(SensorData* pSensorData)
{
LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);
// verify number of range readings in LaserRangeScan matches the number of expected range readings
// if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
// {
// std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
// << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
// return false;
// }
return true;
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
void ParameterManager::Clear()
{
forEach(karto::ParameterVector, &m_Parameters)
{
delete *iter;
}
m_Parameters.clear();
m_ParameterLookup.clear();
}
void ParameterManager::Add(AbstractParameter* pParameter)
{
if (pParameter != NULL && pParameter->GetName() != "")
{
if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
{
m_Parameters.push_back(pParameter);
m_ParameterLookup[pParameter->GetName()] = pParameter;
}
else
{
m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());
assert(false);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/*
std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
{
"Custom",
"Sick_LMS100",
"Sick_LMS200",
"Sick_LMS291",
"Hokuyo_UTM_30LX",
"Hokuyo_URG_04LX"
};
*/
} // namespace karto

3346
Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Mapper.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

106
Localizations/Packages/slam_toolbox/slam_toolbox/package.xml Normal file
View File

@@ -0,0 +1,106 @@
<?xml version="1.0"?>
<package format="2">
<name>slam_toolbox</name>
<description>
This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
</description>
<version>1.5.7</version>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<author>Steve Macenski</author>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>slam_toolbox_msgs</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>eigen</build_depend>
<build_depend>message_filters</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>rosconsole</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sparse_bundle_adjustment</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>interactive_markers</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>suitesparse</build_depend>
<build_depend>liblapack-dev</build_depend>
<build_depend>libceres-dev</build_depend>
<build_depend>libg2o</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_depend>tbb</build_depend>
<build_depend>libqt5-core</build_depend>
<build_depend>libqt5-widgets</build_depend>
<build_depend>qtbase5-dev</build_depend>
<build_depend>map_server</build_depend>
<build_depend>karto_sdk</build_depend>
<build_export_depend>slam_toolbox_msgs</build_export_depend>
<build_export_depend>cmake_modules</build_export_depend>
<build_export_depend>pluginlib</build_export_depend>
<build_export_depend>eigen</build_export_depend>
<build_export_depend>message_filters</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>rosconsole</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sparse_bundle_adjustment</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>tf2_ros</build_export_depend>
<build_export_depend>tf</build_export_depend>
<build_export_depend>tf2</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<build_export_depend>std_srvs</build_export_depend>
<build_export_depend>interactive_markers</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>suitesparse</build_export_depend>
<build_export_depend>liblapack-dev</build_export_depend>
<build_export_depend>libceres-dev</build_export_depend>
<build_export_depend>libg2o</build_export_depend>
<build_export_depend>tf2_geometry_msgs</build_export_depend>
<build_export_depend>tbb</build_export_depend>
<build_export_depend>libqt5-core</build_export_depend>
<build_export_depend>libqt5-widgets</build_export_depend>
<build_export_depend>qtbase5-dev</build_export_depend>
<build_export_depend>map_server</build_export_depend>
<build_export_depend>karto_sdk</build_export_depend>
<exec_depend>slam_toolbox_msgs</exec_depend>
<exec_depend>eigen</exec_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>message_filters</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>rosconsole</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sparse_bundle_adjustment</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>tf2</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>interactive_markers</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>suitesparse</exec_depend>
<exec_depend>liblapack-dev</exec_depend>
<exec_depend>libceres-dev</exec_depend>
<exec_depend>libg2o</exec_depend>
<exec_depend>tf2_geometry_msgs</exec_depend>
<exec_depend>tbb</exec_depend>
<exec_depend>libqt5-core</exec_depend>
<exec_depend>libqt5-widgets</exec_depend>
<exec_depend>map_server</exec_depend>
<exec_depend>karto_sdk</exec_depend>
<test_depend>gtest</test_depend>
<export>
<slam_toolbox plugin="${prefix}/solver_plugins.xml" />
</export>
</package>

21
Localizations/Packages/slam_toolbox/slam_toolbox/snap/generate_snap Normal file
View File

@@ -0,0 +1,21 @@
#!/bin/bash
# make workspace
mkdir -p snap_ws/src
cd snap_ws && catkin_init_workspace
cd src
# add all the necessary things to it
git clone -b melodic-devel https://github.com/SteveMacenski/slam_toolbox.git
cd ../
# move snap and hooks to right place
mkdir snap
cp -r src/slam_toolbox/snap/snapcraft.yaml snap
# build the snap
export SNAPCRAFT_BUILD_ENVIRONMENT_MEMORY=4g
snapcraft
# go back to root
cd ../

47
Localizations/Packages/slam_toolbox/slam_toolbox/snap/snapcraft.yaml Normal file
View File

@@ -0,0 +1,47 @@
name: slam-toolbox-melodic
version: '1.1.3'
summary: Slam Toolbox
description: |
A toolbox for building high quality maps of large spaces with a LIDAR.
grade: stable
confinement: classic
base: core18
type: app
apps:
slam-toolbox-online-sync:
command: roslaunch --wait slam_toolbox online_sync.launch
environment:
LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas)
plugs: [network, network-bind]
slam-toolbox-online-async:
command: roslaunch --wait slam_toolbox online_async.launch
environment:
LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas)
plugs: [network, network-bind]
slam-toolbox-offline:
command: roslaunch --wait slam_toolbox offline.launch
environment:
LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas)
plugs: [network, network-bind]
slam-toolbox-localization:
command: roslaunch --wait slam_toolbox localization.launch
environment:
LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas)
plugs: [network, network-bind]
parts:
workspace:
build-packages: [qt5-default, libgoogle-glog-dev, liblapack-dev, libblas-dev, libpthread-stubs0-dev]
stage-packages: [qt5-default, libgoogle-glog-dev, liblapack-dev, libblas-dev, libpthread-stubs0-dev]
plugin: catkin
source-space: src
include-roscore: true
source: .
catkin-ros-master-uri: http://localhost:11311
catkin-packages: [slam_toolbox]
recursive-rosinstall: false

23
Localizations/Packages/slam_toolbox/slam_toolbox/solver_plugins.xml Normal file
View File

@@ -0,0 +1,23 @@
<library path="libspa_solver_plugin">
<class type="solver_plugins::SpaSolver" base_class_type="karto::ScanSolver">
<description> SPA Optimizer for karto </description>
</class>
</library>
<library path="libg2O_solver_plugin">
<class type="solver_plugins::G2OSolver" base_class_type="karto::ScanSolver">
<description> G2O Optimizer for karto </description>
</class>
</library>
<library path="libceres_solver_plugin">
<class type="solver_plugins::CeresSolver" base_class_type="karto::ScanSolver">
<description> Ceres Optimizer for karto </description>
</class>
</library>
<!-- <library path="libGTSAM_solver_plugin">
<class type="solver_plugins::GTSAMSolver" base_class_type="karto::ScanSolver">
<description> GTSAM Optimizer for karto </description>
</class>
</library> -->

443
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.cpp Normal file
View File

@@ -0,0 +1,443 @@
/*
* Copyright 2018 Simbe Robotics, Inc.
* Author: Steve Macenski (stevenmacenski@gmail.com)
*/
#include "ceres_solver.hpp"
#include <karto_sdk/Karto.h>
#include "ros/console.h"
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(solver_plugins::CeresSolver, karto::ScanSolver)
namespace solver_plugins
{
/*****************************************************************************/
CeresSolver::CeresSolver() :
nodes_(new std::unordered_map<int, Eigen::Vector3d>()),
blocks_(new std::unordered_map<std::size_t,
ceres::ResidualBlockId>()),
problem_(NULL), was_constant_set_(false)
/*****************************************************************************/
{
ros::NodeHandle nh("~");
std::string solver_type, preconditioner_type, dogleg_type,
trust_strategy, loss_fn, mode;
nh.getParam("ceres_linear_solver", solver_type);
nh.getParam("ceres_preconditioner", preconditioner_type);
nh.getParam("ceres_dogleg_type", dogleg_type);
nh.getParam("ceres_trust_strategy", trust_strategy);
nh.getParam("ceres_loss_function", loss_fn);
nh.getParam("mode", mode);
nh.getParam("debug_logging", debug_logging_);
corrections_.clear();
first_node_ = nodes_->end();
// formulate problem
angle_local_parameterization_ = AngleLocalParameterization::Create();
// choose loss function default squared loss (NULL)
loss_function_ = NULL;
if (loss_fn == "HuberLoss")
{
ROS_INFO("CeresSolver: Using HuberLoss loss function.");
loss_function_ = new ceres::HuberLoss(0.7);
}
else if (loss_fn == "CauchyLoss")
{
ROS_INFO("CeresSolver: Using CauchyLoss loss function.");
loss_function_ = new ceres::CauchyLoss(0.7);
}
// choose linear solver default CHOL
options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
if (solver_type == "SPARSE_SCHUR")
{
ROS_INFO("CeresSolver: Using SPARSE_SCHUR solver.");
options_.linear_solver_type = ceres::SPARSE_SCHUR;
}
else if (solver_type == "ITERATIVE_SCHUR")
{
ROS_INFO("CeresSolver: Using ITERATIVE_SCHUR solver.");
options_.linear_solver_type = ceres::ITERATIVE_SCHUR;
}
else if (solver_type == "CGNR")
{
ROS_INFO("CeresSolver: Using CGNR solver.");
options_.linear_solver_type = ceres::CGNR;
}
// choose preconditioner default Jacobi
options_.preconditioner_type = ceres::JACOBI;
if (preconditioner_type == "IDENTITY")
{
ROS_INFO("CeresSolver: Using IDENTITY preconditioner.");
options_.preconditioner_type = ceres::IDENTITY;
}
else if (preconditioner_type == "SCHUR_JACOBI")
{
ROS_INFO("CeresSolver: Using SCHUR_JACOBI preconditioner.");
options_.preconditioner_type = ceres::SCHUR_JACOBI;
}
if (options_.preconditioner_type == ceres::CLUSTER_JACOBI ||
options_.preconditioner_type == ceres::CLUSTER_TRIDIAGONAL)
{
//default canonical view is O(n^2) which is unacceptable for
// problems of this size
options_.visibility_clustering_type = ceres::SINGLE_LINKAGE;
}
// choose trust region strategy default LM
options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
if (trust_strategy == "DOGLEG")
{
ROS_INFO("CeresSolver: Using DOGLEG trust region strategy.");
options_.trust_region_strategy_type = ceres::DOGLEG;
}
// choose dogleg type default traditional
if(options_.trust_region_strategy_type == ceres::DOGLEG)
{
options_.dogleg_type = ceres::TRADITIONAL_DOGLEG;
if (dogleg_type == "SUBSPACE_DOGLEG")
{
ROS_INFO("CeresSolver: Using SUBSPACE_DOGLEG dogleg type.");
options_.dogleg_type = ceres::SUBSPACE_DOGLEG;
}
}
// a typical ros map is 5cm, this is 0.001, 50x the resolution
options_.function_tolerance = 1e-3;
options_.gradient_tolerance = 1e-6;
options_.parameter_tolerance = 1e-3;
options_.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options_.max_num_consecutive_invalid_steps = 3;
options_.max_consecutive_nonmonotonic_steps =
options_.max_num_consecutive_invalid_steps;
options_.num_threads = 50;
options_.use_nonmonotonic_steps = true;
options_.jacobi_scaling = true;
options_.min_relative_decrease = 1e-3;
options_.initial_trust_region_radius = 1e4;
options_.max_trust_region_radius = 1e8;
options_.min_trust_region_radius = 1e-16;
options_.min_lm_diagonal = 1e-6;
options_.max_lm_diagonal = 1e32;
if(options_.linear_solver_type == ceres::SPARSE_NORMAL_CHOLESKY)
{
options_.dynamic_sparsity = true;
}
if (mode == std::string("localization"))
{
// doubles the memory footprint, but lets us remove contraints faster
options_problem_.enable_fast_removal = true;
}
problem_ = new ceres::Problem(options_problem_);
return;
}
/*****************************************************************************/
CeresSolver::~CeresSolver()
/*****************************************************************************/
{
if ( loss_function_ != NULL)
{
delete loss_function_;
}
if (nodes_ != NULL)
{
delete nodes_;
}
if (problem_ != NULL)
{
delete problem_;
}
}
/*****************************************************************************/
void CeresSolver::Compute()
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
if (nodes_->size() == 0)
{
ROS_ERROR("CeresSolver: Ceres was called when there are no nodes."
" This shouldn't happen.");
return;
}
// populate contraint for static initial pose
if (!was_constant_set_ && first_node_ != nodes_->end())
{
ROS_DEBUG("CeresSolver: Setting first node as a constant pose:"
"%0.2f, %0.2f, %0.2f.", first_node_->second(0),
first_node_->second(1), first_node_->second(2));
problem_->SetParameterBlockConstant(&first_node_->second(0));
problem_->SetParameterBlockConstant(&first_node_->second(1));
problem_->SetParameterBlockConstant(&first_node_->second(2));
was_constant_set_ = !was_constant_set_;
}
const ros::Time start_time = ros::Time::now();
ceres::Solver::Summary summary;
ceres::Solve(options_, problem_, &summary);
if (debug_logging_)
{
std::cout << summary.FullReport() << '\n';
}
if (!summary.IsSolutionUsable())
{
ROS_WARN("CeresSolver: "
"Ceres could not find a usable solution to optimize.");
return;
}
// store corrected poses
if (!corrections_.empty())
{
corrections_.clear();
}
corrections_.reserve(nodes_->size());
karto::Pose2 pose;
ConstGraphIterator iter = nodes_->begin();
for ( iter; iter != nodes_->end(); ++iter )
{
pose.SetX(iter->second(0));
pose.SetY(iter->second(1));
pose.SetHeading(iter->second(2));
corrections_.push_back(std::make_pair(iter->first, pose));
}
return;
}
/*****************************************************************************/
const karto::ScanSolver::IdPoseVector& CeresSolver::GetCorrections() const
/*****************************************************************************/
{
return corrections_;
}
/*****************************************************************************/
void CeresSolver::Clear()
/*****************************************************************************/
{
corrections_.clear();
}
/*****************************************************************************/
void CeresSolver::Reset()
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
corrections_.clear();
was_constant_set_ = false;
if (problem_)
{
delete problem_;
}
if (nodes_)
{
delete nodes_;
}
if (blocks_)
{
delete blocks_;
}
nodes_ = new std::unordered_map<int, Eigen::Vector3d>();
blocks_ = new std::unordered_map<std::size_t, ceres::ResidualBlockId>();
problem_ = new ceres::Problem(options_problem_);
first_node_ = nodes_->end();
angle_local_parameterization_ = AngleLocalParameterization::Create();
}
/*****************************************************************************/
void CeresSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
/*****************************************************************************/
{
// store nodes
if (!pVertex)
{
return;
}
karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
Eigen::Vector3d pose2d(pose.GetX(), pose.GetY(), pose.GetHeading());
const int id = pVertex->GetObject()->GetUniqueId();
boost::mutex::scoped_lock lock(nodes_mutex_);
nodes_->insert(std::pair<int,Eigen::Vector3d>(id,pose2d));
if (nodes_->size() == 1)
{
first_node_ = nodes_->find(id);
}
}
/*****************************************************************************/
void CeresSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
/*****************************************************************************/
{
// get IDs in graph for this edge
boost::mutex::scoped_lock lock(nodes_mutex_);
if (!pEdge)
{
return;
}
const int node1 = pEdge->GetSource()->GetObject()->GetUniqueId();
GraphIterator node1it = nodes_->find(node1);
const int node2 = pEdge->GetTarget()->GetObject()->GetUniqueId();
GraphIterator node2it = nodes_->find(node2);
if (node1it == nodes_->end() ||
node2it == nodes_->end() || node1it == node2it)
{
ROS_WARN("CeresSolver: Failed to add constraint, could not find nodes.");
return;
}
// extract transformation
karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
karto::Pose2 diff = pLinkInfo->GetPoseDifference();
Eigen::Vector3d pose2d(diff.GetX(), diff.GetY(), diff.GetHeading());
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
Eigen::Matrix3d information;
information(0, 0) = precisionMatrix(0, 0);
information(0, 1) = information(1, 0) = precisionMatrix(0, 1);
information(0, 2) = information(2, 0) = precisionMatrix(0, 2);
information(1, 1) = precisionMatrix(1, 1);
information(1, 2) = information(2, 1) = precisionMatrix(1, 2);
information(2, 2) = precisionMatrix(2, 2);
Eigen::Matrix3d sqrt_information = information.llt().matrixU();
// populate residual and parameterization for heading normalization
ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(pose2d(0),
pose2d(1), pose2d(2), sqrt_information);
ceres::ResidualBlockId block = problem_->AddResidualBlock(
cost_function, loss_function_,
&node1it->second(0), &node1it->second(1), &node1it->second(2),
&node2it->second(0), &node2it->second(1), &node2it->second(2));
problem_->SetParameterization(&node1it->second(2),
angle_local_parameterization_);
problem_->SetParameterization(&node2it->second(2),
angle_local_parameterization_);
blocks_->insert(std::pair<std::size_t, ceres::ResidualBlockId>(
GetHash(node1, node2), block));
return;
}
/*****************************************************************************/
void CeresSolver::RemoveNode(kt_int32s id)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
GraphIterator nodeit = nodes_->find(id);
if (nodeit != nodes_->end())
{
if (problem_->HasParameterBlock(&nodeit->second(0)) &&
problem_->HasParameterBlock(&nodeit->second(1)) &&
problem_->HasParameterBlock(&nodeit->second(2)))
{
problem_->RemoveParameterBlock(&nodeit->second(0));
problem_->RemoveParameterBlock(&nodeit->second(1));
problem_->RemoveParameterBlock(&nodeit->second(2));
ROS_DEBUG("RemoveNode: Removed node id %d", nodeit->first);
}
else
{
ROS_DEBUG("RemoveNode: Failed to remove parameter blocks for node id %d", nodeit->first);
}
nodes_->erase(nodeit);
}
else
{
ROS_ERROR("RemoveNode: Failed to find node matching id %i", (int)id);
}
}
/*****************************************************************************/
void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
std::unordered_map<std::size_t, ceres::ResidualBlockId>::iterator it_a =
blocks_->find(GetHash(sourceId, targetId));
std::unordered_map<std::size_t, ceres::ResidualBlockId>::iterator it_b =
blocks_->find(GetHash(targetId, sourceId));
if (it_a != blocks_->end())
{
problem_->RemoveResidualBlock(it_a->second);
blocks_->erase(it_a);
}
else if (it_b != blocks_->end())
{
problem_->RemoveResidualBlock(it_b->second);
blocks_->erase(it_b);
}
else
{
ROS_ERROR("RemoveConstraint: Failed to find residual block for %i %i",
(int)sourceId, (int)targetId);
}
}
/*****************************************************************************/
void CeresSolver::ModifyNode(const int& unique_id, Eigen::Vector3d pose)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
GraphIterator it = nodes_->find(unique_id);
if (it != nodes_->end())
{
double yaw_init = it->second(2);
it->second = pose;
it->second(2) += yaw_init;
}
}
/*****************************************************************************/
void CeresSolver::GetNodeOrientation(const int& unique_id, double& pose)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
GraphIterator it = nodes_->find(unique_id);
if (it != nodes_->end())
{
pose = it->second(2);
}
}
/*****************************************************************************/
std::unordered_map<int, Eigen::Vector3d>* CeresSolver::getGraph()
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(nodes_mutex_);
return nodes_;
}
} // end namespace

72
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.hpp Normal file
View File

@@ -0,0 +1,72 @@
/*
* Copyright 2018 Simbe Robotics, Inc.
* Author: Steve Macenski (stevenmacenski@gmail.com)
*/
#ifndef KARTO_CERESSOLVER_H
#define KARTO_CERESSOLVER_H
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <vector>
#include <unordered_map>
#include <utility>
#include <karto_sdk/Mapper.h>
#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>
#include <cmath>
#include <math.h>
#include "../include/slam_toolbox/toolbox_types.hpp"
#include "ceres_utils.h"
namespace solver_plugins
{
using namespace ::toolbox_types;
class CeresSolver : public karto::ScanSolver
{
public:
CeresSolver();
virtual ~CeresSolver();
public:
virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; //Get corrected poses after optimization
virtual void Compute(); //Solve
virtual void Clear(); //Resets the corrections
virtual void Reset(); //Resets the solver plugin clean
virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex); //Adds a node to the solver
virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge); //Adds a constraint to the solver
virtual std::unordered_map<int, Eigen::Vector3d>* getGraph(); //Get graph stored
virtual void RemoveNode(kt_int32s id); //Removes a node from the solver correction table
virtual void RemoveConstraint(kt_int32s sourceId, kt_int32s targetId); // Removes constraints from the optimization problem
virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose); // change a node's pose
virtual void GetNodeOrientation(const int& unique_id, double& pose); // get a node's current pose yaw
private:
// karto
karto::ScanSolver::IdPoseVector corrections_;
// ceres
ceres::Solver::Options options_;
ceres::Problem::Options options_problem_;
ceres::LossFunction* loss_function_;
ceres::Problem* problem_;
ceres::LocalParameterization* angle_local_parameterization_;
bool was_constant_set_, debug_logging_;
// graph
std::unordered_map<int, Eigen::Vector3d>* nodes_;
std::unordered_map<size_t, ceres::ResidualBlockId>* blocks_;
std::unordered_map<int, Eigen::Vector3d>::iterator first_node_;
boost::mutex nodes_mutex_;
};
}
#endif

103
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_utils.h Normal file
View File

@@ -0,0 +1,103 @@
/*
* Copyright 2018 Simbe Robotics
* Author: Steve Macenski
*/
#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>
#include <cmath>
#include <utility>
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
inline std::size_t GetHash(const int& x, const int& y)
{
return ((std::hash<double>()(x) ^ (std::hash<double>()(y) << 1)) >> 1);
}
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
// Normalizes the angle in radians between [-pi and pi).
template <typename T> inline T NormalizeAngle(const T& angle_radians)
{
T two_pi(2.0 * M_PI);
return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
class AngleLocalParameterization
{
public:
template <typename T>
bool operator()(const T* theta_radians, const T* delta_theta_radians, T* theta_radians_plus_delta) const
{
*theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians);
return true;
}
static ceres::LocalParameterization* Create()
{
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
}
};
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians)
{
const T cos_yaw = ceres::cos(yaw_radians);
const T sin_yaw = ceres::sin(yaw_radians);
Eigen::Matrix<T, 2, 2> rotation;
rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
return rotation;
}
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
class PoseGraph2dErrorTerm
{
public:
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information)
{
}
template <typename T>
bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a, const T* const x_b, const T* const y_b, const T* const yaw_b, T* residuals_ptr) const
{
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction* Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information)
{
return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};

186
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.cpp Normal file
View File

@@ -0,0 +1,186 @@
/*********************************************************************
*
* Copyright (c) 2017, Saurav Agarwal
* All rights reserved.
*
*********************************************************************/
/* Authors: Saurav Agarwal */
/* Modified: Steve Macenski */
#include <limits>
#include <karto_sdk/Karto.h>
#include <ros/console.h>
#include "GTSAM_solver.hpp"
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(solver_plugins::GTSAMSolver, karto::ScanSolver)
namespace solver_plugins
{
/*****************************************************************************/
GTSAMSolver::GTSAMSolver()
/*****************************************************************************/
{
using namespace gtsam;
// add the prior on the first node which is known
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
graph_.emplace_shared<PriorFactor<Pose2> >(0, Pose2(0, 0, 0), priorNoise);
}
/*****************************************************************************/
GTSAMSolver::~GTSAMSolver()
/*****************************************************************************/
{
}
/*****************************************************************************/
void GTSAMSolver::Clear()
/*****************************************************************************/
{
corrections_.clear();
}
/*****************************************************************************/
const karto::ScanSolver::IdPoseVector& GTSAMSolver::GetCorrections() const
/*****************************************************************************/
{
return corrections_;
}
/*****************************************************************************/
void GTSAMSolver::Compute()
/*****************************************************************************/
{
using namespace gtsam;
corrections_.clear();
graphNodes_.clear();
LevenbergMarquardtParams parameters;
// Stop iterating once the change in error between steps is less than this value
parameters.relativeErrorTol = 1e-5;
// Do not perform more than N iteration steps
parameters.maxIterations = 500;
// Create the optimizer ...
LevenbergMarquardtOptimizer optimizer(graph_, initialGuess_, parameters);
// ... and optimize
Values result = optimizer.optimize();
Values::ConstFiltered<Pose2> viewPose2 = result.filter<Pose2>();
// put values into corrections container
for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2)
{
karto::Pose2 pose(key_value.value.x(), key_value.value.y(),
key_value.value.theta());
corrections_.push_back(std::make_pair(key_value.key, pose));
graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(),
key_value.value.y()));
}
}
/*****************************************************************************/
void GTSAMSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
/*****************************************************************************/
{
using namespace gtsam;
karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
initialGuess_.insert(pVertex->GetObject()->GetUniqueId(),
Pose2( odom.GetX(), odom.GetY(), odom.GetHeading() ));
graphNodes_.push_back(Eigen::Vector2d(odom.GetX(), odom.GetY()));
ROS_DEBUG("[gtsam] Adding node %d.", pVertex->GetObject()->GetUniqueId());
}
/*****************************************************************************/
void GTSAMSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
/*****************************************************************************/
{
using namespace gtsam;
// Set source and target
int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
// Set the measurement (poseGraphEdge distance between vertices)
karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
karto::Pose2 diff = pLinkInfo->GetPoseDifference();
// Set the covariance of the measurement
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance();
Eigen::Matrix<double,3,3> cov;
cov(0,0) = precisionMatrix(0,0);
cov(0,1) = cov(1,0) = precisionMatrix(0,1);
cov(0,2) = cov(2,0) = precisionMatrix(0,2);
cov(1,1) = precisionMatrix(1,1);
cov(1,2) = cov(2,1) = precisionMatrix(1,2);
cov(2,2) = precisionMatrix(2,2);
noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov);
// Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph_.emplace_shared<BetweenFactor<Pose2> >(sourceID, targetID,
Pose2(diff.GetX(), diff.GetY(), diff.GetHeading()), model);
// Add the constraint to the optimizer
ROS_DEBUG("[gtsam] Adding Edge from node %d to node %d.", sourceID, targetID);
}
/*****************************************************************************/
void GTSAMSolver::getGraph(std::vector<Eigen::Vector2d>& nodes)
/*****************************************************************************/
{
nodes = graphNodes_;
// using namespace gtsam;
// double *data1 = new double[3];
// double *data2 = new double[3];
// for (SparseOptimizer::EdgeSet::iterator it =
// optimizer_.edges().begin(); it != optimizer_.edges().end(); ++it)
// {
// EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
// if(e)
// {
// VertexSE2* v1 = dynamic_cast<VertexSE2*>(e->vertices()[0]);
// v1->getEstimateData(data1);
// Eigen::Vector2d poseFrom(data1[0], data1[1]);
// VertexSE2* v2 = dynamic_cast<VertexSE2*>(e->vertices()[1]);
// v2->getEstimateData(data2);
// Eigen::Vector2d poseTo(data2[0], data2[1]);
// edges.push_back(std::make_pair(poseFrom, poseTo));
// }
// }
// delete data1;
// delete data2;
}
} // end namespace

93
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.hpp Normal file
View File

@@ -0,0 +1,93 @@
/*********************************************************************
*
* Copyright (c) 2017, Saurav Agarwal
* All rights reserved.
*
*********************************************************************/
/* Authors: Saurav Agarwal */
/* Modified: Steve Macenski */
#ifndef KARTO_GTSAMSolver_H
#define KARTO_GTSAMSolver_H
#include <karto_sdk/Mapper.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
namespace solver_plugins
{
/**
* @brief Wrapper for G2O to interface with Open Karto
*/
class GTSAMSolver : public karto::ScanSolver
{
public:
GTSAMSolver();
virtual ~GTSAMSolver();
public:
/**
* @brief Clear the vector of corrections
* @details Empty out previously computed corrections
*/
virtual void Clear();
/**
* @brief Solve the SLAM back-end
* @details Calls G2O to solve the SLAM back-end
*/
virtual void Compute();
/**
* @brief Get the vector of corrections
* @details Get the vector of corrections
* @return Vector with corrected poses
*/
virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
/**
* @brief Add a node to pose-graph
* @details Add a node which is a robot pose to the pose-graph
*
* @param pVertex the node to be added in
*/
virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex);
/**
* @brief Add an edge constraint to pose-graph
* @details Adds a relative pose measurement constraint between two poses in the graph
*
* @param pEdge [description]
*/
virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge);
/**
* @brief Get the pose-graph
* @details Get the underlying graph from g2o, return the graph of constraints
*
* @param g the graph
*/
virtual void getGraph(std::vector<Eigen::Vector2d>& nodes); // std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d> > &edges);
virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose
private:
karto::ScanSolver::IdPoseVector corrections_;
gtsam::NonlinearFactorGraph graph_;
gtsam::Values initialGuess_;
std::vector<Eigen::Vector2d> graphNodes_;
};
} // end namespace
#endif // KARTO_GTSAMSolver_H

234
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.cpp Normal file
View File

@@ -0,0 +1,234 @@
/*********************************************************************
*
* Copyright (c) 2017, Saurav Agarwal
* All rights reserved.
* Modified: Steve Macenski (stevenmacenski@gmail.com)
*
*********************************************************************/
#include "g2o_solver.hpp"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include <karto_sdk/Karto.h>
#include <ros/console.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(solver_plugins::G2OSolver, karto::ScanSolver)
namespace solver_plugins
{
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
//typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
/*****************************************************************************/
G2OSolver::G2OSolver()
/*****************************************************************************/
{
// Initialize the SparseOptimizer
auto linearSolver = g2o::make_unique<SlamLinearSolver>();
linearSolver->setBlockOrdering(false);
auto blockSolver = g2o::make_unique<SlamBlockSolver>(
std::move(linearSolver));
optimizer_.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
std::move(blockSolver)));
latestNodeID_ = 0;
useRobustKernel_ = true;
}
/*****************************************************************************/
G2OSolver::~G2OSolver()
/*****************************************************************************/
{
// destroy all the singletons
g2o::Factory::destroy();
g2o::OptimizationAlgorithmFactory::destroy();
g2o::HyperGraphActionLibrary::destroy();
}
/*****************************************************************************/
void G2OSolver::Clear()
/*****************************************************************************/
{
corrections_.clear();
}
const karto::ScanSolver::IdPoseVector& G2OSolver::GetCorrections() const
{
return corrections_;
}
/*****************************************************************************/
void G2OSolver::Compute()
/*****************************************************************************/
{
corrections_.clear();
// Fix the first node in the graph to hold the map in place
g2o::OptimizableGraph::Vertex* first = optimizer_.vertex(0);
if(!first)
{
ROS_ERROR("[g2o] No Node with ID 0 found!");
return;
}
first->setFixed(true);
// Do the graph optimization
const ros::Time start_time = ros::Time::now();
optimizer_.initializeOptimization();
int iter = optimizer_.optimize(500);
ROS_INFO("Loop Closure Solve time: %f seconds",
(ros::Time::now() - start_time).toSec());
if (iter <= 0)
{
ROS_ERROR("[g2o] Optimization failed, result might be invalid!");
return;
}
// Write the result so it can be used by the mapper
g2o::SparseOptimizer::VertexContainer nodes = optimizer_.activeVertices();
for (g2o::SparseOptimizer::VertexContainer::const_iterator n =
nodes.begin(); n != nodes.end(); n++)
{
double estimate[3];
if((*n)->getEstimateData(estimate))
{
karto::Pose2 pose(estimate[0], estimate[1], estimate[2]);
corrections_.push_back(std::make_pair((*n)->id(), pose));
}
else
{
ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!");
}
}
}
/*****************************************************************************/
void G2OSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
/*****************************************************************************/
{
karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
g2o::VertexSE2* poseVertex = new g2o::VertexSE2;
poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(),
odom.GetHeading()));
poseVertex->setId(pVertex->GetObject()->GetUniqueId());
optimizer_.addVertex(poseVertex);
latestNodeID_ = pVertex->GetObject()->GetUniqueId();
ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId());
}
/*****************************************************************************/
void G2OSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
/*****************************************************************************/
{
// Create a new edge
g2o::EdgeSE2* odometry = new g2o::EdgeSE2;
// Set source and target
int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
odometry->vertices()[0] = optimizer_.vertex(sourceID);
odometry->vertices()[1] = optimizer_.vertex(targetID);
if(odometry->vertices()[0] == NULL)
{
ROS_ERROR("[g2o] Source vertex with id %d does not exist!", sourceID);
delete odometry;
return;
}
if(odometry->vertices()[0] == NULL)
{
ROS_ERROR("[g2o] Target vertex with id %d does not exist!", targetID);
delete odometry;
return;
}
// Set the measurement (odometry distance between vertices)
karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
karto::Pose2 diff = pLinkInfo->GetPoseDifference();
g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading());
odometry->setMeasurement(measurement);
// Set the covariance of the measurement
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
Eigen::Matrix<double,3,3> info;
info(0,0) = precisionMatrix(0,0);
info(0,1) = info(1,0) = precisionMatrix(0,1);
info(0,2) = info(2,0) = precisionMatrix(0,2);
info(1,1) = precisionMatrix(1,1);
info(1,2) = info(2,1) = precisionMatrix(1,2);
info(2,2) = precisionMatrix(2,2);
odometry->setInformation(info);
if(useRobustKernel_)
{
g2o::RobustKernelDCS* rk = new g2o::RobustKernelDCS;
odometry->setRobustKernel(rk);
}
// Add the constraint to the optimizer
ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID);
optimizer_.addEdge(odometry);
}
/*****************************************************************************/
void G2OSolver::getGraph(std::vector<Eigen::Vector2d>& nodes)
/*****************************************************************************/
{
double *data = new double[3];
for (g2o::SparseOptimizer::VertexIDMap::iterator it =
optimizer_.vertices().begin(); it != optimizer_.vertices().end(); ++it)
{
g2o::VertexSE2* v = dynamic_cast<g2o::VertexSE2*>(it->second);
if(v)
{
v->getEstimateData(data);
Eigen::Vector2d pose(data[0], data[1]);
nodes.push_back(pose);
}
}
delete data;
//using namespace g2o;
//HyperGraph::VertexIDMap vertexMap = optimizer_.vertices();
//HyperGraph::EdgeSet edgeSet = optimizer_.edges();
// double *data1 = new double[3];
// double *data2 = new double[3];
// for (SparseOptimizer::EdgeSet::iterator it = optimizer_.edges().begin();
// it != optimizer_.edges().end(); ++it)
// {
// EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
// if(e)
// {
// VertexSE2* v1 = dynamic_cast<VertexSE2*>(e->vertices()[0]);
// v1->getEstimateData(data1);
// Eigen::Vector2d poseFrom(data1[0], data1[1]);
// VertexSE2* v2 = dynamic_cast<VertexSE2*>(e->vertices()[1]);
// v2->getEstimateData(data2);
// Eigen::Vector2d poseTo(data2[0], data2[1]);
// edges.push_back(std::make_pair(poseFrom, poseTo));
// }
// }
// delete data1;
// delete data2;
}
} // end namespace

102
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.hpp Normal file
View File

@@ -0,0 +1,102 @@
/*********************************************************************
*
* Copyright (c) 2017, Saurav Agarwal
* All rights reserved.
*
*********************************************************************/
#ifndef KARTO_G2OSolver_H
#define KARTO_G2OSolver_H
#include <karto_sdk/Mapper.h>
#include "g2o/core/sparse_optimizer.h"
namespace solver_plugins
{
/**
* @brief Wrapper for G2O to interface with Open Karto
*/
class G2OSolver : public karto::ScanSolver
{
public:
G2OSolver();
virtual ~G2OSolver();
public:
/**
* @brief Clear the vector of corrections
* @details Empty out previously computed corrections
*/
virtual void Clear();
/**
* @brief Solve the SLAM back-end
* @details Calls G2O to solve the SLAM back-end
*/
virtual void Compute();
/**
* @brief Get the vector of corrections
* @details Get the vector of corrections
* @return Vector with corrected poses
*/
virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
/**
* @brief Add a node to pose-graph
* @details Add a node which is a robot pose to the pose-graph
*
* @param pVertex the node to be added in
*/
virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex);
/**
* @brief Add an edge constraint to pose-graph
* @details Adds a relative pose measurement constraint between two poses in the graph
*
* @param pEdge [description]
*/
virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge);
/**
* @brief Get the pose-graph
* @details Get the underlying graph from g2o, return the graph of constraints
*
* @param g the graph
*/
void getGraph(std::vector<Eigen::Vector2d>& nodes); // std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d> > &edges);
/**
* @brief Use robust kernel in back-end
* @details Uses Dynamic Covariance scaling kernel in back-end
*
* @param flag variable, if true robust kernel will be used
*/
void useRobustKernel(bool flag)
{
useRobustKernel_ = flag;
}
virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose
private:
karto::ScanSolver::IdPoseVector corrections_;
g2o::SparseOptimizer optimizer_;
int latestNodeID_; // ID of the latest added node, this is used internally in AddHeadingConstraint
bool useRobustKernel_;
};
}
#endif // KARTO_G2OSolver_H

126
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.cpp Normal file
View File

@@ -0,0 +1,126 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "spa_solver.hpp"
#include <karto_sdk/Karto.h>
#include "ros/console.h"
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(solver_plugins::SpaSolver, karto::ScanSolver)
namespace solver_plugins
{
typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
/*****************************************************************************/
SpaSolver::SpaSolver()
/*****************************************************************************/
{
}
/*****************************************************************************/
SpaSolver::~SpaSolver()
/*****************************************************************************/
{
}
/*****************************************************************************/
void SpaSolver::Clear()
/*****************************************************************************/
{
corrections.clear();
}
const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const
{
return corrections;
}
/*****************************************************************************/
void SpaSolver::Compute()
/*****************************************************************************/
{
corrections.clear();
const ros::Time start_time = ros::Time::now();
m_Spa.doSPA(40, 1.0e-4, 1);
ROS_INFO("Loop Closure Solve time: %f seconds",
(ros::Time::now() - start_time).toSec());
NodeVector nodes = m_Spa.getNodes();
forEach(NodeVector, &nodes)
{
karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
corrections.push_back(std::make_pair(iter->nodeId, pose));
}
}
/*****************************************************************************/
void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
/*****************************************************************************/
{
karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
}
/*****************************************************************************/
void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
/*****************************************************************************/
{
karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
karto::Pose2 diff = pLinkInfo->GetPoseDifference();
Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
Eigen::Matrix<double,3,3> m;
m(0,0) = precisionMatrix(0,0);
m(0,1) = m(1,0) = precisionMatrix(0,1);
m(0,2) = m(2,0) = precisionMatrix(0,2);
m(1,1) = precisionMatrix(1,1);
m(1,2) = m(2,1) = precisionMatrix(1,2);
m(2,2) = precisionMatrix(2,2);
m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}
/*****************************************************************************/
void SpaSolver::getGraph(std::vector<Eigen::Vector2d>& g)
/*****************************************************************************/
{
std::vector<float> raw_graph;
m_Spa.getGraph(raw_graph);
g.reserve(raw_graph.size()/4);
Eigen::Vector2d pose;
for (size_t i=0; i!=raw_graph.size()/4; i++)
{
pose(0) = raw_graph[4*i];
pose(1) = raw_graph[4*i+1];
g.push_back(pose);
}
}
} // end namespace

61
Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.hpp Normal file
View File

@@ -0,0 +1,61 @@
/*
* Copyright 2010 SRI International
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KARTO_SPASOLVER_H
#define KARTO_SPASOLVER_H
#include <karto_sdk/Mapper.h>
#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR
#include <Eigen/Eigen>
#include <sparse_bundle_adjustment/spa2d.h>
namespace solver_plugins
{
class SpaSolver : public karto::ScanSolver
{
public:
SpaSolver();
virtual ~SpaSolver();
public:
virtual void Clear();
virtual void Compute();
virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex);
virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge);
virtual void getGraph(std::vector<Eigen::Vector2d>& g);
virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose);
private:
karto::ScanSolver::IdPoseVector corrections;
sba::SysSPA2d m_Spa;
};
}
#endif // KARTO_SPASOLVER_H

444
Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp Normal file
View File

@@ -0,0 +1,444 @@
/*
* slam_toolbox
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp"
namespace slam_toolbox
{
/*****************************************************************************/
void LifelongSlamToolbox::checkIsNotNormalized(const double& value)
/*****************************************************************************/
{
if (value < 0.0 || value > 1.0)
{
ROS_FATAL("All stores and scales must be in range [0, 1].");
exit(-1);
}
}
/*****************************************************************************/
LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh)
: SlamToolbox(nh)
/*****************************************************************************/
{
loadPoseGraphByParams(nh);
nh.param("lifelong_search_use_tree", use_tree_, false);
nh.param("lifelong_minimum_score", iou_thresh_, 0.10);
nh.param("lifelong_iou_match", iou_match_, 0.85);
nh.param("lifelong_node_removal_score", removal_score_, 0.10);
nh.param("lifelong_overlap_score_scale", overlap_scale_, 0.5);
nh.param("lifelong_constraint_multiplier", constraint_scale_, 0.05);
nh.param("lifelong_nearby_penalty", nearby_penalty_, 0.001);
nh.param("lifelong_candidates_scale", candidates_scale_, 0.03);
checkIsNotNormalized(iou_thresh_);
checkIsNotNormalized(constraint_scale_);
checkIsNotNormalized(removal_score_);
checkIsNotNormalized(overlap_scale_);
checkIsNotNormalized(iou_match_);
checkIsNotNormalized(nearby_penalty_);
checkIsNotNormalized(candidates_scale_);
ROS_WARN("Lifelong mapping mode in SLAM Toolbox is considered "
"experimental and should be understood before proceeding. Please visit: "
"https://github.com/SteveMacenski/slam_toolbox/wiki/"
"Experimental-Lifelong-Mapping-Node for more information.");
// in lifelong mode, we cannot have interactive mode enabled
enable_interactive_mode_ = false;
}
/*****************************************************************************/
void LifelongSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
{
return;
}
// ensure the laser can be used
LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN_THROTTLE(5., "Failed to create laser device for"
" %s; discarding scan", scan->header.frame_id.c_str());
return;
}
// LTS additional bounded node increase parameter (rate, or total for run or at all?)
// LTS pseudo-localization mode. If want to add a scan, but not deleting a scan, add to local buffer?
// LTS if (eval() && dont_add_more_scans) {addScan()} else {localization_add_scan()}
// LTS if (eval() && ctr / total < add_rate_scans) {addScan()} else {localization_add_scan()}
karto::LocalizedRangeScan* range_scan = addScan(laser, scan, pose);
evaluateNodeDepreciation(range_scan);
return;
}
/*****************************************************************************/
void LifelongSlamToolbox::evaluateNodeDepreciation(
LocalizedRangeScan* range_scan)
/*****************************************************************************/
{
if (range_scan)
{
boost::mutex::scoped_lock lock(smapper_mutex_);
const BoundingBox2& bb = range_scan->GetBoundingBox();
const Size2<double> bb_size = bb.GetSize();
double radius = sqrt(bb_size.GetWidth()*bb_size.GetWidth() +
bb_size.GetHeight()*bb_size.GetHeight()) / 2.0;
Vertices near_scan_vertices = FindScansWithinRadius(range_scan, radius);
ScoredVertices scored_verices =
computeScores(near_scan_vertices, range_scan);
ScoredVertices::iterator it;
for (it = scored_verices.begin(); it != scored_verices.end(); ++it)
{
if (it->GetScore() < removal_score_)
{
ROS_INFO("Removing node %i from graph with score: %f and "
"old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(),
it->GetScore(), it->GetVertex()->GetScore());
removeFromSlamGraph(it->GetVertex());
}
else
{
updateScoresSlamGraph(it->GetScore(), it->GetVertex());
}
}
}
return;
}
/*****************************************************************************/
Vertices LifelongSlamToolbox::FindScansWithinRadius(
LocalizedRangeScan* scan, const double& radius)
/*****************************************************************************/
{
// Using the tree will create a Kd-tree and find all neighbors in graph
// around the reference scan. Otherwise it will use the graph and
// access scans within radius that are connected with constraints to this
// node.
if (use_tree_)
{
return
smapper_->getMapper()->GetGraph()->FindNearByVertices(
scan->GetSensorName(), scan->GetBarycenterPose(), radius);
}
else
{
return
smapper_->getMapper()->GetGraph()->FindNearLinkedVertices(scan, radius);
}
}
/*****************************************************************************/
double LifelongSlamToolbox::computeObjectiveScore(
const double& intersect_over_union,
const double& area_overlap,
const double& reading_overlap,
const int& num_constraints,
const double& initial_score,
const int& num_candidates) const
/*****************************************************************************/
{
// We have some useful metrics. lets make a new score
// intersect_over_union: The higher this score, the better aligned in scope these scans are
// area_overlap: The higher, the more area they share normalized by candidate size
// reading_overlap: The higher, the more readings of the new scan the candidate contains
// num_constraints: The lower, the less other nodes may rely on this candidate
// initial_score: Last score of this vertex before update
// this is a really good fit and not from a loop closure, lets just decay
if (intersect_over_union > iou_match_ && num_constraints < 3)
{
return -1.0;
}
// to be conservative, lets say the overlap is the lesser of the
// area and proportion of laser returns in the intersecting region.
double overlap = overlap_scale_ * std::min(area_overlap, reading_overlap);
// if the num_constraints are high we want to stave off the decay, but not override it
double contraint_scale_factor = std::min(1.0,
std::max(0., constraint_scale_ * (num_constraints - 2)));
contraint_scale_factor = std::min(contraint_scale_factor, overlap);
//
double candidates = num_candidates - 1;
double candidate_scale_factor = candidates_scale_ * candidates;
// Give the initial score a boost proportional to the number of constraints
// Subtract the overlap amount, apply a penalty for just being nearby
// and scale the entire additional score by the number of candidates
double score =
initial_score * (1.0 + contraint_scale_factor)
- overlap
- nearby_penalty_;
//score += (initial_score - score) * candidate_scale_factor;
if (score > 1.0)
{
ROS_ERROR("Objective function calculated for vertex score (%0.4f)"
" greater than one! Thresholding to 1.0", score);
return 1.0;
}
return score;
}
/*****************************************************************************/
double LifelongSlamToolbox::computeScore(
LocalizedRangeScan* reference_scan,
Vertex<LocalizedRangeScan>* candidate,
const double& initial_score, const int& num_candidates)
/*****************************************************************************/
{
double new_score = initial_score;
LocalizedRangeScan* candidate_scan = candidate->GetObject();
// compute metrics for information loss normalized
double iou = computeIntersectOverUnion(reference_scan, candidate_scan);
double area_overlap = computeAreaOverlapRatio(reference_scan, candidate_scan);
int num_constraints = candidate->GetEdges().size();
double reading_overlap = computeReadingOverlapRatio(reference_scan, candidate_scan);
bool critical_lynchpoint = candidate_scan->GetUniqueId() == 0 ||
candidate_scan->GetUniqueId() == 1;
int id_diff = reference_scan->GetUniqueId() - candidate_scan->GetUniqueId();
if (id_diff < smapper_->getMapper()->getParamScanBufferSize() ||
critical_lynchpoint)
{
return initial_score;
}
double score = computeObjectiveScore(iou,
area_overlap,
reading_overlap,
num_constraints,
initial_score,
num_candidates);
ROS_INFO("Metric Scores: Initial: %f, IOU: %f,"
" Area: %f, Num Con: %i, Reading: %f, outcome score: %f.",
initial_score, iou, area_overlap, num_constraints, reading_overlap, score);
return score;
}
/*****************************************************************************/
ScoredVertices
LifelongSlamToolbox::computeScores(
Vertices& near_scans,
LocalizedRangeScan* range_scan)
/*****************************************************************************/
{
ScoredVertices scored_vertices;
scored_vertices.reserve(near_scans.size());
// must have some minimum metric to utilize
// IOU will drop sharply with fitment, I'd advise not setting this value
// any higher than 0.15. Also check this is a linked constraint
// We want to do this early to get a better estimate of local candidates
ScanVector::iterator candidate_scan_it;
double iou = 0.0;
for (candidate_scan_it = near_scans.begin();
candidate_scan_it != near_scans.end(); )
{
iou = computeIntersectOverUnion(range_scan,
(*candidate_scan_it)->GetObject());
if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2)
{
candidate_scan_it = near_scans.erase(candidate_scan_it);
}
else
{
++candidate_scan_it;
}
}
for (candidate_scan_it = near_scans.begin();
candidate_scan_it != near_scans.end(); ++candidate_scan_it)
{
ScoredVertex scored_vertex((*candidate_scan_it),
computeScore(range_scan, (*candidate_scan_it),
(*candidate_scan_it)->GetScore(), near_scans.size()));
scored_vertices.push_back(scored_vertex);
}
return scored_vertices;
}
/*****************************************************************************/
void LifelongSlamToolbox::removeFromSlamGraph(
Vertex<LocalizedRangeScan>* vertex)
/*****************************************************************************/
{
smapper_->getMapper()->RemoveNodeFromGraph(vertex);
smapper_->getMapper()->GetMapperSensorManager()->RemoveScan(
vertex->GetObject());
dataset_->RemoveData(vertex->GetObject());
vertex->RemoveObject();
delete vertex;
vertex = nullptr;
// LTS what do we do about the contraints that node had about it?Nothing?Transfer?
}
/*****************************************************************************/
void LifelongSlamToolbox::updateScoresSlamGraph(const double& score,
Vertex<LocalizedRangeScan>* vertex)
/*****************************************************************************/
{
// Saved in graph so it persists between sessions and runs
vertex->SetScore(score);
}
/*****************************************************************************/
bool LifelongSlamToolbox::deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
/*****************************************************************************/
{
if (req.match_type == procType::LOCALIZE_AT_POSE)
{
ROS_ERROR("Requested a localization deserialization "
"in non-localization mode.");
return false;
}
return SlamToolbox::deserializePoseGraphCallback(req, resp);
}
/*****************************************************************************/
void LifelongSlamToolbox::computeIntersectBounds(
LocalizedRangeScan* s1, LocalizedRangeScan* s2,
double& x_l, double& x_u, double& y_l, double& y_u)
/*****************************************************************************/
{
Size2<double> bb1 = s1->GetBoundingBox().GetSize();
Size2<double> bb2 = s2->GetBoundingBox().GetSize();
Pose2 pose1 = s1->GetBarycenterPose();
Pose2 pose2 = s2->GetBarycenterPose();
const double s1_upper_x = pose1.GetX() + (bb1.GetWidth() / 2.0);
const double s1_upper_y = pose1.GetY() + (bb1.GetHeight() / 2.0);
const double s1_lower_x = pose1.GetX() - (bb1.GetWidth() / 2.0);
const double s1_lower_y = pose1.GetY() - (bb1.GetHeight() / 2.0);
const double s2_upper_x = pose2.GetX() + (bb2.GetWidth() / 2.0);
const double s2_upper_y = pose2.GetY() + (bb2.GetHeight() / 2.0);
const double s2_lower_x = pose2.GetX() - (bb2.GetWidth() / 2.0);
const double s2_lower_y = pose2.GetY() - (bb2.GetHeight() / 2.0);
x_u = std::min(s1_upper_x, s2_upper_x);
y_u = std::min(s1_upper_y, s2_upper_y);
x_l = std::max(s1_lower_x, s2_lower_x);
y_l = std::max(s1_lower_y, s2_lower_y);
return;
}
/*****************************************************************************/
double LifelongSlamToolbox::computeIntersect(LocalizedRangeScan* s1,
LocalizedRangeScan* s2)
/*****************************************************************************/
{
double x_l, x_u, y_l, y_u;
computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u);
const double intersect = (y_u - y_l) * (x_u - x_l);
if (intersect < 0.0)
{
return 0.0;
}
return intersect;
}
/*****************************************************************************/
double LifelongSlamToolbox::computeIntersectOverUnion(LocalizedRangeScan* s1,
LocalizedRangeScan* s2)
/*****************************************************************************/
{
// this is a common metric in machine learning used to determine
// the fitment of a set of bounding boxes. Its response sharply
// drops by box matches.
const double intersect = computeIntersect(s1, s2);
Size2<double> bb1 = s1->GetBoundingBox().GetSize();
Size2<double> bb2 = s2->GetBoundingBox().GetSize();
const double uni = (bb1.GetWidth() * bb1.GetHeight()) +
(bb2.GetWidth() * bb2.GetHeight()) - intersect;
return intersect / uni;
}
/*****************************************************************************/
double LifelongSlamToolbox::computeAreaOverlapRatio(
LocalizedRangeScan* ref_scan,
LocalizedRangeScan* candidate_scan)
/*****************************************************************************/
{
// ref scan is new scan, candidate scan is potential for decay
// so we want to find the ratio of space of the candidate scan
// the reference scan takes up
double overlap_area = computeIntersect(ref_scan, candidate_scan);
Size2<double> bb_candidate = candidate_scan->GetBoundingBox().GetSize();
const double candidate_area =
bb_candidate.GetHeight() * bb_candidate.GetWidth();
return overlap_area / candidate_area;
}
/*****************************************************************************/
double LifelongSlamToolbox::computeReadingOverlapRatio(
LocalizedRangeScan* ref_scan,
LocalizedRangeScan* candidate_scan)
/*****************************************************************************/
{
const PointVectorDouble& pts = candidate_scan->GetPointReadings(true);
const int num_pts = pts.size();
// get the bounds of the intersect area
double x_l, x_u, y_l, y_u;
computeIntersectBounds(ref_scan, candidate_scan, x_l, x_u, y_l, y_u);
PointVectorDouble::const_iterator pt_it;
int inner_pts = 0;
for (pt_it = pts.begin(); pt_it != pts.end(); ++pt_it)
{
if (pt_it->GetX() < x_u && pt_it->GetX() > x_l &&
pt_it->GetY() < y_u && pt_it->GetY() > y_l)
{
inner_pts++;
}
}
return double(inner_pts) / double(num_pts);
}
} // end namespace

45
Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong_node.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*
* slam_toolbox
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp"
int main(int argc, char** argv)
{
ros::init(argc, argv, "slam_toolbox");
ros::NodeHandle nh("~");
ros::spinOnce();
int stack_size;
if (nh.getParam("stack_size_to_use", stack_size))
{
ROS_INFO("Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
getrlimit(RLIMIT_STACK, &stack_limit);
if (stack_limit.rlim_cur < stack_size)
{
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
}
slam_toolbox::LifelongSlamToolbox llst(nh);
ros::spin();
return 0;
}

230
Localizations/Packages/slam_toolbox/slam_toolbox/src/laser_utils.cpp Normal file
View File

@@ -0,0 +1,230 @@
/*
* laser_utils
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/laser_utils.hpp"
#include <cmath>
namespace laser_utils
{
LaserMetadata::LaserMetadata()
{
};
LaserMetadata::~LaserMetadata()
{
}
LaserMetadata::LaserMetadata(karto::LaserRangeFinder* lsr, bool invert)
{
laser = lsr;
inverted = invert;
};
bool LaserMetadata::isInverted() const
{
return inverted;
}
karto::LaserRangeFinder* LaserMetadata::getLaser()
{
return laser;
}
void LaserMetadata::invertScan(sensor_msgs::LaserScan& scan) const
{
sensor_msgs::LaserScan temp;
temp.intensities.reserve(scan.intensities.size());
temp.ranges.reserve(scan.ranges.size());
const bool has_intensities = scan.intensities.size() > 0 ? true : false;
for (int i = scan.ranges.size(); i != 0; i--)
{
temp.ranges.push_back(scan.ranges[i]);
if (has_intensities)
{
temp.intensities.push_back(scan.intensities[i]);
}
}
scan.ranges = temp.ranges;
scan.intensities = temp.intensities;
return;
};
LaserAssistant::LaserAssistant(ros::NodeHandle& nh,
tf2_ros::Buffer* tf, const std::string& base_frame)
: nh_(nh), tf_(tf), base_frame_(base_frame)
{
};
LaserAssistant::~LaserAssistant()
{
};
LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::LaserScan scan)
{
scan_ = scan;
frame_ = scan_.header.frame_id;
double mountingYaw;
bool inverted = isInverted(mountingYaw);
karto::LaserRangeFinder* laser = makeLaser(mountingYaw);
LaserMetadata laserMeta(laser, inverted);
return laserMeta;
};
karto::LaserRangeFinder* LaserAssistant::makeLaser(const double& mountingYaw)
{
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(
karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar"));
laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x,
laser_pose_.transform.translation.y, mountingYaw));
laser->SetMinimumRange(scan_.range_min);
laser->SetMaximumRange(scan_.range_max);
laser->SetMinimumAngle(scan_.angle_min);
laser->SetMaximumAngle(scan_.angle_max);
laser->SetAngularResolution(scan_.angle_increment);
bool is_360_lidar = false;
const float angular_range = std::fabs(scan_.angle_max - scan_.angle_min);
if (std::fabs(angular_range - 2.0 * M_PI) < (scan_.angle_increment - (std::numeric_limits<float>::epsilon() * 2.0f*M_PI))) {
is_360_lidar = true;
}
// Check if we have a 360 laser, but incorrectly setup as to produce
// measurements in range [0, 360] rather than appropriately as [0, 360)
if (angular_range > 6.10865 /*350 deg*/ && (angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) {
is_360_lidar = false;
}
laser->SetIs360Laser(is_360_lidar);
double max_laser_range = 25;
nh_.getParam("max_laser_range", max_laser_range);
if (max_laser_range > scan_.range_max)
{
ROS_WARN("maximum laser range setting (%.1f m) exceeds the capabilities "
"of the used Lidar (%.1f m)",
max_laser_range, scan_.range_max);
max_laser_range = scan_.range_max;
}
laser->SetRangeThreshold(max_laser_range);
return laser;
}
bool LaserAssistant::isInverted(double& mountingYaw)
{
// geometry_msgs::TransformStamped laser_ident;
// laser_ident.header.stamp = scan_.header.stamp;
// laser_ident.header.frame_id = frame_;
// laser_ident.transform.rotation.w = 1.0;
// laser_pose_ = tf_->transform(laser_ident, base_frame_);
// mountingYaw = tf2::getYaw(laser_pose_.transform.rotation);
// ROS_DEBUG("laser %s's pose wrt base: %.3f %.3f %.3f %.3f",
// frame_.c_str(), laser_pose_.transform.translation.x,
// laser_pose_.transform.translation.y,
// laser_pose_.transform.translation.z, mountingYaw);
// geometry_msgs::Vector3Stamped laser_orient;
// laser_orient.vector.z = laser_orient.vector.y = 0.;
// laser_orient.vector.z = 1 + laser_pose_.transform.translation.z;
// laser_orient.header.stamp = scan_.header.stamp;
// laser_orient.header.frame_id = base_frame_;
// laser_orient = tf_->transform(laser_orient, frame_);
// if (laser_orient.vector.z <= 0)
// {
// ROS_DEBUG("laser is mounted upside-down");
// return true;
// }
bool result = false;
try
{
laser_pose_ = tf_->lookupTransform(base_frame_, frame_, ros::Time(0));
mountingYaw = tf2::getYaw(laser_pose_.transform.rotation);
ROS_INFO("Transform from %s to %s: x=%f, y=%f, z=%f, yaw=%f",
frame_.c_str(),
base_frame_.c_str(),
laser_pose_.transform.translation.x,
laser_pose_.transform.translation.y,
laser_pose_.transform.translation.z,
mountingYaw);
auto quaternion = laser_pose_.transform.rotation;
// ROS_INFO("Quaternion: x=%f, y=%f, z=%f, w=%f",
// quaternion.x, quaternion.y, quaternion.z, quaternion.w);
tf2::Quaternion q(
quaternion.x,
quaternion.y,
quaternion.z,
quaternion.w
);
double roll, pitch, yaw;
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
ROS_INFO("Laser orientation: Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw);
if (abs(pitch) > 3.13 && abs(pitch) < 3.15)
{
ROS_WARN("Laser is mounted upside-down");
result = true;
}
else
{
ROS_INFO("Laser orientation seems to be correct.");
result = false;
}
}
catch (tf2::TransformException &ex)
{
ROS_WARN("[TransformException] Could not get transform: %s", ex.what());
}
return result;
};
ScanHolder::ScanHolder(std::map<std::string, laser_utils::LaserMetadata>& lasers)
: lasers_(lasers)
{
current_scans_ = std::make_unique<std::vector<sensor_msgs::LaserScan> >();
};
ScanHolder::~ScanHolder()
{
current_scans_.reset();
};
sensor_msgs::LaserScan ScanHolder::getCorrectedScan(const int& id)
{
sensor_msgs::LaserScan scan = current_scans_->at(id);
const laser_utils::LaserMetadata& laser = lasers_[scan.header.frame_id];
if (laser.isInverted())
{
laser.invertScan(scan);
}
return scan;
};
void ScanHolder::addScan(const sensor_msgs::LaserScan scan)
{
current_scans_->push_back(scan);
};
} // end namespace

365
Localizations/Packages/slam_toolbox/slam_toolbox/src/loop_closure_assistant.cpp Normal file
View File

@@ -0,0 +1,365 @@
/*
* loop_closure_assistant
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/loop_closure_assistant.hpp"
namespace loop_closure_assistant
{
/*****************************************************************************/
LoopClosureAssistant::LoopClosureAssistant(
ros::NodeHandle& node,
karto::Mapper* mapper,
laser_utils::ScanHolder* scan_holder,
PausedState& state, ProcessType & processor_type)
: mapper_(mapper), scan_holder_(scan_holder),
interactive_mode_(false), nh_(node), state_(state),
processor_type_(processor_type)
/*****************************************************************************/
{
node.setParam("paused_processing", false);
tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
ssClear_manual_ = node.advertiseService("clear_changes",
&LoopClosureAssistant::clearChangesCallback, this);
ssLoopClosure_ = node.advertiseService("manual_loop_closure",
&LoopClosureAssistant::manualLoopClosureCallback, this);
scan_publisher_ = node.advertise<sensor_msgs::LaserScan>(
"karto_scan_visualization",10);
solver_ = mapper_->getScanSolver();
interactive_server_ =
std::make_unique<interactive_markers::InteractiveMarkerServer>(
"slam_toolbox","",true);
ssInteractive_ = node.advertiseService("toggle_interactive_mode",
&LoopClosureAssistant::interactiveModeCallback,this);
node.setParam("interactive_mode", interactive_mode_);
marker_publisher_ = node.advertise<visualization_msgs::MarkerArray>(
"karto_graph_visualization",1);
node.param("map_frame", map_frame_, std::string("map"));
node.param("enable_interactive_mode", enable_interactive_mode_, false);
}
/*****************************************************************************/
void LoopClosureAssistant::setMapper(karto::Mapper * mapper)
/*****************************************************************************/
{
mapper_ = mapper;
}
/*****************************************************************************/
void LoopClosureAssistant::processInteractiveFeedback(const
visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
/*****************************************************************************/
{
if (processor_type_ != PROCESS)
{
ROS_ERROR_THROTTLE(5.,
"Interactive mode is invalid outside processing mode.");
return;
}
const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1;
// was depressed, something moved, and now released
if (feedback->event_type ==
visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP &&
feedback->mouse_point_valid)
{
addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x,
feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation)));
}
// is currently depressed, being moved before release
if (feedback->event_type ==
visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
{
// get scan
sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id);
// get correct orientation
tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0);
double node_yaw, first_node_yaw;
solver_->GetNodeOrientation(id, node_yaw);
solver_->GetNodeOrientation(0, first_node_yaw);
tf2::Quaternion q1(0.,0.,0.,1.0);
q1.setEuler(0., 0., node_yaw - 3.14159);
tf2::Quaternion q2(0.,0.,0.,1.0);
q2.setEuler(0., 0., 3.14159);
quat *= q1;
quat *= q2;
// interactive move
tf2::convert(feedback->pose.orientation, msg_quat);
quat *= msg_quat;
quat.normalize();
// create correct transform
tf2::Transform transform;
transform.setOrigin(tf2::Vector3(feedback->pose.position.x,
feedback->pose.position.y, 0.));
transform.setRotation(quat);
// publish the scan visualization with transform
geometry_msgs::TransformStamped msg;
tf2::convert(transform, msg.transform);
msg.child_frame_id = "karto_scan_visualization";
msg.header.frame_id = feedback->header.frame_id;
msg.header.stamp = ros::Time::now();
tfB_->sendTransform(msg);
scan.header.frame_id = "karto_scan_visualization";
scan.header.stamp = ros::Time::now();
scan_publisher_.publish(scan);
}
}
/*****************************************************************************/
void LoopClosureAssistant::publishGraph()
/*****************************************************************************/
{
interactive_server_->clear();
karto::MapperGraph * graph = mapper_->GetGraph();
if (!graph || graph->GetVertices().empty())
{
return;
}
using ConstVertexMapIterator =
karto::MapperGraph::VertexMap::const_iterator;
const karto::MapperGraph::VertexMap& vertices = graph->GetVertices();
int graph_size = 0;
for (ConstVertexMapIterator it = vertices.begin(); it != vertices.end(); ++it)
{
graph_size += it->second.size();
}
ROS_DEBUG("Graph size: %i", graph_size);
bool interactive_mode = false;
{
boost::mutex::scoped_lock lock(interactive_mutex_);
interactive_mode = interactive_mode_;
}
const size_t current_marker_count = marker_array_.markers.size();
marker_array_.markers.clear(); // restart the marker count
visualization_msgs::Marker vertex_marker =
vis_utils::toVertexMarker(map_frame_, "slam_toolbox", 0.1);
for (ConstVertexMapIterator outer_it = vertices.begin();
outer_it != vertices.end(); ++outer_it)
{
using ConstVertexMapValueIterator =
karto::MapperGraph::VertexMap::mapped_type::const_iterator;
for (ConstVertexMapValueIterator inner_it = outer_it->second.begin();
inner_it != outer_it->second.end(); ++inner_it)
{
karto::LocalizedRangeScan * scan = inner_it->second->GetObject();
vertex_marker.pose.position.x = scan->GetCorrectedPose().GetX();
vertex_marker.pose.position.y = scan->GetCorrectedPose().GetY();
if (interactive_mode && enable_interactive_mode_)
{
// need a 1-to-1 mapping between marker IDs and
// scan unique IDs to process interactive feedback
vertex_marker.id = scan->GetUniqueId() + 1;
visualization_msgs::InteractiveMarker int_marker =
vis_utils::toInteractiveMarker(vertex_marker, 0.3);
interactive_server_->insert(int_marker,
boost::bind(
&LoopClosureAssistant::processInteractiveFeedback,
this, _1));
}
else
{
// use monotonically increasing vertex marker IDs to
// make room for edge marker IDs
vertex_marker.id = marker_array_.markers.size();
marker_array_.markers.push_back(vertex_marker);
}
}
}
if (!interactive_mode)
{
using EdgeList = std::vector<karto::Edge<karto::LocalizedRangeScan>*>;
using ConstEdgeListIterator = EdgeList::const_iterator;
visualization_msgs::Marker edge_marker =
vis_utils::toEdgeMarker(map_frame_, "slam_toolbox", 0.05);
const EdgeList& edges = graph->GetEdges();
for (ConstEdgeListIterator it = edges.begin(); it != edges.end(); ++it)
{
const karto::Edge<karto::LocalizedRangeScan> * edge = *it;
karto::LocalizedRangeScan * source_scan = edge->GetSource()->GetObject();
karto::LocalizedRangeScan * target_scan = edge->GetTarget()->GetObject();
const karto::Pose2 source_pose = source_scan->GetCorrectedPose();
const karto::Pose2 target_pose = target_scan->GetCorrectedPose();
edge_marker.id = marker_array_.markers.size();
edge_marker.points[0].x = source_pose.GetX();
edge_marker.points[0].y = source_pose.GetY();
edge_marker.points[1].x = target_pose.GetX();
edge_marker.points[1].y = target_pose.GetY();
marker_array_.markers.push_back(edge_marker);
}
}
const size_t next_marker_count = marker_array_.markers.size();
// append preexisting markers to force deletion
while (marker_array_.markers.size() < current_marker_count)
{
visualization_msgs::Marker deleted_marker;
deleted_marker.id = marker_array_.markers.size();
deleted_marker.action = visualization_msgs::Marker::DELETE;
marker_array_.markers.push_back(deleted_marker);
}
// if disabled, clears out old markers
interactive_server_->applyChanges();
marker_publisher_.publish(marker_array_);
// drop trailing deleted markers
marker_array_.markers.resize(next_marker_count);
return;
}
/*****************************************************************************/
bool LoopClosureAssistant::manualLoopClosureCallback(
slam_toolbox_msgs::LoopClosure::Request& req,
slam_toolbox_msgs::LoopClosure::Response& resp)
/*****************************************************************************/
{
if(!enable_interactive_mode_)
{
ROS_WARN("Called manual loop closure"
" with interactive mode disabled. Ignoring.");
return false;
}
{
boost::mutex::scoped_lock lock(moved_nodes_mutex_);
if (moved_nodes_.size() == 0)
{
ROS_WARN("No moved nodes to attempt manual loop closure.");
return true;
}
ROS_INFO("LoopClosureAssistant: Attempting to manual "
"loop close with %i moved nodes.", (int)moved_nodes_.size());
// for each in node map
std::map<int, Eigen::Vector3d>::const_iterator it = moved_nodes_.begin();
for (it; it != moved_nodes_.end(); ++it)
{
moveNode(it->first,
Eigen::Vector3d(it->second(0),it->second(1), it->second(2)));
}
}
// optimize
mapper_->CorrectPoses();
// update visualization and clear out nodes completed
publishGraph();
clearMovedNodes();
return true;
}
/*****************************************************************************/
bool LoopClosureAssistant::interactiveModeCallback(
slam_toolbox_msgs::ToggleInteractive::Request &req,
slam_toolbox_msgs::ToggleInteractive::Response &resp)
/*****************************************************************************/
{
if(!enable_interactive_mode_)
{
ROS_WARN("Called toggle interactive mode with "
"interactive mode disabled. Ignoring.");
return false;
}
bool interactive_mode;
{
boost::mutex::scoped_lock lock_i(interactive_mutex_);
interactive_mode_ = !interactive_mode_;
interactive_mode = interactive_mode_;
nh_.setParam("interactive_mode", interactive_mode_);
}
ROS_INFO("SlamToolbox: Toggling %s interactive mode.",
interactive_mode ? "on" : "off");
publishGraph();
clearMovedNodes();
// set state so we don't overwrite changes in rviz while loop closing
state_.set(PROCESSING, interactive_mode);
state_.set(VISUALIZING_GRAPH, interactive_mode);
nh_.setParam("paused_processing", interactive_mode);
return true;
}
/*****************************************************************************/
void LoopClosureAssistant::moveNode(
const int& id, const Eigen::Vector3d& pose)
/*****************************************************************************/
{
solver_->ModifyNode(id, pose);
}
/*****************************************************************************/
bool LoopClosureAssistant::clearChangesCallback(
slam_toolbox_msgs::Clear::Request& req,
slam_toolbox_msgs::Clear::Response& resp)
/*****************************************************************************/
{
if(!enable_interactive_mode_)
{
ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring.");
return false;
}
ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes.");
publishGraph();
clearMovedNodes();
return true;
}
/*****************************************************************************/
void LoopClosureAssistant::clearMovedNodes()
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(moved_nodes_mutex_);
moved_nodes_.clear();
}
/*****************************************************************************/
void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec)
/*****************************************************************************/
{
ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure "
"pose has been recorded.",id);
boost::mutex::scoped_lock lock(moved_nodes_mutex_);
moved_nodes_[id] = vec;
}
} // end namespace

68
Localizations/Packages/slam_toolbox/slam_toolbox/src/map_saver.cpp Normal file
View File

@@ -0,0 +1,68 @@
/*
* map_saver
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/map_saver.hpp"
namespace map_saver
{
/*****************************************************************************/
MapSaver::MapSaver(ros::NodeHandle & nh, const std::string& map_name)
: nh_(nh), map_name_(map_name), received_map_(false)
/*****************************************************************************/
{
server_ = nh_.advertiseService("save_map", &MapSaver::saveMapCallback, this);
sub_ = nh_.subscribe(map_name_, 1, &MapSaver::mapCallback, this);
}
/*****************************************************************************/
void MapSaver::mapCallback(const nav_msgs::OccupancyGrid& msg)
/*****************************************************************************/
{
received_map_ = true;
}
/*****************************************************************************/
bool MapSaver::saveMapCallback(
slam_toolbox_msgs::SaveMap::Request& req,
slam_toolbox_msgs::SaveMap::Response& resp)
/*****************************************************************************/
{
if (!received_map_)
{
ROS_WARN("Map Saver: Cannot save map, no map yet received on topic %s.",
map_name_.c_str());
return false;
}
const std::string name = req.name.data;
if (name != "")
{
ROS_INFO("SlamToolbox: Saving map as %s.", name.c_str());
int rc = system(("rosrun map_server map_saver -f " + name).c_str());
}
else
{
ROS_INFO("SlamToolbox: Saving map in current directory.");
int rc = system("rosrun map_server map_saver");
}
ros::Duration(1.0).sleep();
return true;
}
} // end namespace

367
Localizations/Packages/slam_toolbox/slam_toolbox/src/merge_maps_kinematic.cpp Normal file
View File

@@ -0,0 +1,367 @@
/*
* Author
* Copyright (c) 2018, Simbe Robotics, Inc.
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/merge_maps_kinematic.hpp"
#include "slam_toolbox/serialization.hpp"
/*****************************************************************************/
MergeMapsKinematic::MergeMapsKinematic() : num_submaps_(0), nh_("map_merging")
/*****************************************************************************/
{
ROS_INFO("MergeMapsKinematic: Starting up!");
setup();
}
/*****************************************************************************/
void MergeMapsKinematic::setup()
/*****************************************************************************/
{
nh_.param("resolution", resolution_, 0.05);
sstS_.push_back(nh_.advertise<nav_msgs::OccupancyGrid>("/map", 1, true));
sstmS_.push_back(nh_.advertise<nav_msgs::MapMetaData>(
"/map_metadata", 1, true));
ssMap_ = nh_.advertiseService("merge_submaps",
&MergeMapsKinematic::mergeMapCallback, this);
ssSubmap_ = nh_.advertiseService("add_submap",
&MergeMapsKinematic::addSubmapCallback, this);
tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
interactive_server_ =
std::make_unique<interactive_markers::InteractiveMarkerServer>(
"merge_maps_tool","",true);
}
/*****************************************************************************/
MergeMapsKinematic::~MergeMapsKinematic()
/*****************************************************************************/
{
}
/*****************************************************************************/
bool MergeMapsKinematic::addSubmapCallback(
slam_toolbox_msgs::AddSubmap::Request& req,
slam_toolbox_msgs::AddSubmap::Response& resp)
/*****************************************************************************/
{
std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
if (!serialization::read(req.filename, *mapper, *dataset))
{
ROS_ERROR("addSubmapCallback: Failed to read "
"file: %s.", req.filename.c_str());
return true;
}
// we know the position because we put it there before any scans
karto::LaserRangeFinder* laser = dynamic_cast<karto::LaserRangeFinder*>(
dataset->GetLasers()[0]);
dataset->Add(laser, true);
dataset_vec_.push_back(std::move(dataset));
if (lasers_.find(laser->GetName().GetName()) == lasers_.end())
{
laser_utils::LaserMetadata laserMeta(laser, false);
lasers_[laser->GetName().GetName()] = laserMeta;
}
karto::LocalizedRangeScanVector scans = mapper->GetAllProcessedScans();
scans_vec_.push_back(scans);
num_submaps_++;
// create and publish map with marker that will move the map around
sstS_.push_back(nh_.advertise<nav_msgs::OccupancyGrid>(
"/map_"+std::to_string(num_submaps_), 1, true));
sstmS_.push_back(nh_.advertise<nav_msgs::MapMetaData>(
"/map_metadata_" + std::to_string(num_submaps_), 1, true));
sleep(1.0);
nav_msgs::GetMap::Response map;
nav_msgs::OccupancyGrid& og = map.map;
try
{
kartoToROSOccupancyGrid(scans, map);
} catch (const karto::Exception& e)
{
ROS_WARN("Failed to build grid to add submap, Exception: %s",
e.GetErrorMessage().c_str());
return false;
}
tf2::Transform transform;
transform.setIdentity();
transform.setOrigin(tf2::Vector3(og.info.origin.position.x +
og.info.width * og.info.resolution / 2.0,
og.info.origin.position.y + og.info.height * og.info.resolution / 2.0,
0.));
og.info.origin.position.x = - (og.info.width * og.info.resolution / 2.0);
og.info.origin.position.y = - (og.info.height * og.info.resolution / 2.0);
og.header.stamp = ros::Time::now();
og.header.frame_id = "map_"+std::to_string(num_submaps_);
sstS_[num_submaps_].publish(og);
sstmS_[num_submaps_].publish(og.info);
geometry_msgs::TransformStamped msg;
tf2::convert(transform, msg.transform);
msg.child_frame_id = "/map_"+std::to_string(num_submaps_);
msg.header.frame_id = "/map";
msg.header.stamp = ros::Time::now();
tfB_->sendTransform(msg);
submap_marker_transform_[num_submaps_] =
tf2::Transform(tf2::Quaternion(0.,0.,0.,1.0),
tf2::Vector3(0,0,0)); //no initial correction -- identity mat
submap_locations_[num_submaps_] =
Eigen::Vector3d(transform.getOrigin().getX(),
transform.getOrigin().getY(),0.);
// create an interactive marker for the base of this frame and attach it
visualization_msgs::Marker m =
vis_utils::toVertexMarker("map", "merge_maps_tool", 2.0);
m.pose.position.x = transform.getOrigin().getX();
m.pose.position.y = transform.getOrigin().getY();
m.id = num_submaps_;
visualization_msgs::InteractiveMarker int_marker =
vis_utils::toInteractiveMarker(m, 2.4);
interactive_server_->insert(int_marker,
boost::bind(&MergeMapsKinematic::processInteractiveFeedback, this, _1));
interactive_server_->applyChanges();
ROS_INFO("Map %s was loaded into topic %s!", req.filename.c_str(),
("/map_"+std::to_string(num_submaps_)).c_str());
return true;
}
/*****************************************************************************/
karto::Pose2 MergeMapsKinematic::applyCorrection(const
karto::Pose2& pose,
const tf2::Transform& submap_correction)
/*****************************************************************************/
{
tf2::Transform pose_tf, pose_corr;
tf2::Quaternion q(0.,0.,0.,1.0);
q.setRPY(0., 0., pose.GetHeading());
pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
pose_tf.setRotation(q);
pose_corr = submap_correction * pose_tf;
return karto::Pose2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y(),
tf2::getYaw(pose_corr.getRotation()));
}
/*****************************************************************************/
karto::Vector2<kt_double> MergeMapsKinematic::applyCorrection(const
karto::Vector2<kt_double>& pose,
const tf2::Transform& submap_correction)
/*****************************************************************************/
{
tf2::Transform pose_tf, pose_corr;
pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
pose_tf.setRotation(tf2::Quaternion(0.,0.,0.,1.0));
pose_corr = submap_correction * pose_tf;
return karto::Vector2<kt_double>(pose_corr.getOrigin().x(),
pose_corr.getOrigin().y());
}
/*****************************************************************************/
void MergeMapsKinematic::transformScan(LocalizedRangeScansIt iter,
tf2::Transform& submap_correction)
/*****************************************************************************/
{
// TRANSFORM BARYCENTERR POSE
const karto::Pose2 bary_center_pose = (*iter)->GetBarycenterPose();
auto bary_center_pose_corr =
applyCorrection(bary_center_pose, submap_correction);
(*iter)->SetBarycenterPose(bary_center_pose_corr);
// TRANSFORM BOUNDING BOX POSITIONS
karto::BoundingBox2 bbox = (*iter)->GetBoundingBox();
const karto::Vector2<kt_double> bbox_min_corr =
applyCorrection(bbox.GetMinimum(), submap_correction);
bbox.SetMinimum(bbox_min_corr);
const karto::Vector2<kt_double> bbox_max_corr =
applyCorrection(bbox.GetMaximum(), submap_correction);
bbox.SetMaximum(bbox_max_corr);
(*iter)->SetBoundingBox(bbox);
// TRANSFORM UNFILTERED POINTS USED
karto::PointVectorDouble UPR_vec = (*iter)->GetPointReadings();
for(karto::PointVectorDouble::iterator it_upr = UPR_vec.begin();
it_upr != UPR_vec.end(); ++it_upr)
{
const karto::Vector2<kt_double> upr_corr = applyCorrection(
*it_upr, submap_correction);
it_upr->SetX(upr_corr.GetX());
it_upr->SetY(upr_corr.GetY());
}
(*iter)->SetPointReadings(UPR_vec);
// TRANSFORM CORRECTED POSE
const karto::Pose2 corrected_pose = (*iter)->GetCorrectedPose();
karto::Pose2 karto_robot_pose_corr = applyCorrection(
corrected_pose, submap_correction);
(*iter)->SetCorrectedPose(karto_robot_pose_corr);
kt_bool dirty = false;
(*iter)->SetIsDirty(dirty);
// TRANSFORM ODOM POSE
karto::Pose2 odom_pose = (*iter)->GetOdometricPose();
karto::Pose2 karto_robot_pose_odom = applyCorrection(
odom_pose, submap_correction);
(*iter)->SetOdometricPose(karto_robot_pose_odom);
}
/*****************************************************************************/
bool MergeMapsKinematic::mergeMapCallback(
slam_toolbox_msgs::MergeMaps::Request& req,
slam_toolbox_msgs::MergeMaps::Response& resp)
/*****************************************************************************/
{
ROS_INFO("Merging maps!");
// transform all the scans into the new global map coordinates
int id = 0;
karto::LocalizedRangeScanVector transformed_scans;
for(LocalizedRangeScansVecIt it_LRV = scans_vec_.begin();
it_LRV != scans_vec_.end(); ++it_LRV)
{
id++;
for (LocalizedRangeScansIt iter = it_LRV->begin();
iter != it_LRV->end(); ++iter)
{
tf2::Transform submap_correction = submap_marker_transform_[id];
transformScan(iter, submap_correction);
transformed_scans.push_back((*iter));
}
}
// create the map
nav_msgs::GetMap::Response map;
try
{
kartoToROSOccupancyGrid(transformed_scans, map);
} catch (const karto::Exception& e)
{
ROS_WARN("Failed to build grid to merge maps together, Exception: %s",
e.GetErrorMessage().c_str());
}
// publish
map.map.header.stamp = ros::Time::now();
map.map.header.frame_id = "map";
sstS_[0].publish(map.map);
sstmS_[0].publish(map.map.info);
return true;
}
/*****************************************************************************/
void MergeMapsKinematic::kartoToROSOccupancyGrid(
const karto::LocalizedRangeScanVector& scans,
nav_msgs::GetMap::Response& map)
/*****************************************************************************/
{
karto::OccupancyGrid* occ_grid = NULL;
occ_grid = karto::OccupancyGrid::CreateFromScans(scans, resolution_);
if (!occ_grid)
{
ROS_INFO("MergeMapsKinematic: Could not make Karto occupancy grid.");
}
else
{
map.map.info.resolution = resolution_;
vis_utils::toNavMap(occ_grid, map.map);
}
delete occ_grid;
return;
}
/*****************************************************************************/
void MergeMapsKinematic::processInteractiveFeedback(const
visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
/*****************************************************************************/
{
const int id = std::stoi(feedback->marker_name,nullptr,10);
if (feedback->event_type ==
visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP &&
feedback->mouse_point_valid)
{
tfScalar yaw = tf2::getYaw(feedback->pose.orientation);
tf2::Quaternion quat(0.,0.,0.,1.0);
tf2::convert(feedback->pose.orientation, quat); // relative
tf2::Transform previous_submap_correction;
previous_submap_correction.setIdentity();
previous_submap_correction.setOrigin(tf2::Vector3(submap_locations_[id](0),
submap_locations_[id](1), 0.));
// update internal knowledge of submap locations
submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x,
feedback->pose.position.y,
submap_locations_[id](2) + yaw);
// add the map_N frame there
tf2::Transform new_submap_location;
new_submap_location.setOrigin(tf2::Vector3(submap_locations_[id](0),
submap_locations_[id](1), 0.));
new_submap_location.setRotation(quat);
geometry_msgs::TransformStamped msg;
tf2::convert(new_submap_location, msg.transform);
msg.child_frame_id = "/map_"+std::to_string(id);
msg.header.frame_id = "/map";
msg.header.stamp = ros::Time::now();
tfB_->sendTransform(msg);
submap_marker_transform_[id] = submap_marker_transform_[id] *
previous_submap_correction.inverse() * new_submap_location;
}
if (feedback->event_type ==
visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
{
tfScalar yaw = tf2::getYaw(feedback->pose.orientation);
tf2::Quaternion quat(0.,0.,0.,1.0);
tf2::convert(feedback->pose.orientation, quat); // relative
// add the map_N frame there
tf2::Transform new_submap_location;
new_submap_location.setOrigin(tf2::Vector3(feedback->pose.position.x,
feedback->pose.position.y, 0.));
new_submap_location.setRotation(quat);
geometry_msgs::TransformStamped msg;
tf2::convert(new_submap_location, msg.transform);
msg.child_frame_id = "/map_"+std::to_string(id);
msg.header.frame_id = "/map";
msg.header.stamp = ros::Time::now();
tfB_->sendTransform(msg);
}
}
/*****************************************************************************/
int main(int argc, char** argv)
/*****************************************************************************/
{
ros::init(argc, argv, "merge_maps_kinematic");
MergeMapsKinematic mmk;
ros::spin();
return 0;
}

289
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_mapper.cpp Normal file
View File

@@ -0,0 +1,289 @@
/*
* slam_mapper
* Copyright (c) 2018, Simbe Robotics
* Copyright (c) 2018, Steve Macenski
* Copyright (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_mapper.hpp"
namespace mapper_utils
{
/*****************************************************************************/
SMapper::SMapper()
/*****************************************************************************/
{
mapper_ = std::make_unique<karto::Mapper>();
}
/*****************************************************************************/
SMapper::~SMapper()
/*****************************************************************************/
{
mapper_.reset();
}
/*****************************************************************************/
karto::Mapper* SMapper::getMapper()
/*****************************************************************************/
{
return mapper_.get();
}
/*****************************************************************************/
void SMapper::setMapper(karto::Mapper* mapper)
/*****************************************************************************/
{
mapper_.reset(mapper);
}
/*****************************************************************************/
void SMapper::clearLocalizationBuffer()
/*****************************************************************************/
{
mapper_->ClearLocalizationBuffer();
}
/*****************************************************************************/
karto::OccupancyGrid* SMapper::getOccupancyGrid(const double& resolution)
/*****************************************************************************/
{
karto::OccupancyGrid* occ_grid = nullptr;
return karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(),
resolution);
}
/*****************************************************************************/
tf2::Transform SMapper::toTfPose(const karto::Pose2& pose) const
/*****************************************************************************/
{
tf2::Transform new_pose;
new_pose.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
tf2::Quaternion q;
q.setRPY(0., 0., pose.GetHeading());
new_pose.setRotation(q);
return new_pose;
};
/*****************************************************************************/
karto::Pose2 SMapper::toKartoPose(const tf2::Transform& pose) const
/*****************************************************************************/
{
karto::Pose2 transformed_pose;
transformed_pose.SetX(pose.getOrigin().x());
transformed_pose.SetY(pose.getOrigin().y());
transformed_pose.SetHeading(tf2::getYaw(pose.getRotation()));
return transformed_pose;
};
/*****************************************************************************/
void SMapper::configure(const ros::NodeHandle& nh)
/*****************************************************************************/
{
bool use_scan_matching;
if(nh.getParam("use_scan_matching", use_scan_matching))
{
mapper_->setParamUseScanMatching(use_scan_matching);
}
bool use_scan_barycenter;
if(nh.getParam("use_scan_barycenter", use_scan_barycenter))
{
mapper_->setParamUseScanBarycenter(use_scan_barycenter);
}
double minimum_travel_distance = 0.5;
if(nh.getParam("minimum_travel_distance", minimum_travel_distance))
{
mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
}
double minimum_travel_heading;
if(nh.getParam("minimum_travel_heading", minimum_travel_heading))
{
mapper_->setParamMinimumTravelHeading(minimum_travel_heading);
}
int scan_buffer_size;
if(nh.getParam("scan_buffer_size", scan_buffer_size))
{
mapper_->setParamScanBufferSize(scan_buffer_size);
}
double scan_buffer_maximum_scan_distance;
if(nh.getParam("scan_buffer_maximum_scan_distance",
scan_buffer_maximum_scan_distance))
{
mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);
}
double link_match_minimum_response_fine;
if(nh.getParam("link_match_minimum_response_fine",
link_match_minimum_response_fine))
{
mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);
}
double link_scan_maximum_distance;
if(nh.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
{
mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);
}
double loop_search_maximum_distance;
if(nh.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
{
mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);
}
bool do_loop_closing;
if(nh.getParam("do_loop_closing", do_loop_closing))
{
mapper_->setParamDoLoopClosing(do_loop_closing);
}
int loop_match_minimum_chain_size;
if(nh.getParam("loop_match_minimum_chain_size",
loop_match_minimum_chain_size))
{
mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);
}
double loop_match_maximum_variance_coarse;
if(nh.getParam("loop_match_maximum_variance_coarse",
loop_match_maximum_variance_coarse))
{
mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);
}
double loop_match_minimum_response_coarse;
if(nh.getParam("loop_match_minimum_response_coarse",
loop_match_minimum_response_coarse))
{
mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);
}
double loop_match_minimum_response_fine;
if(nh.getParam("loop_match_minimum_response_fine",
loop_match_minimum_response_fine))
{
mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);
}
// Setting Correlation Parameters
double correlation_search_space_dimension;
if(nh.getParam("correlation_search_space_dimension",
correlation_search_space_dimension))
{
mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);
}
double correlation_search_space_resolution;
if(nh.getParam("correlation_search_space_resolution",
correlation_search_space_resolution))
{
mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);
}
double correlation_search_space_smear_deviation;
if(nh.getParam("correlation_search_space_smear_deviation",
correlation_search_space_smear_deviation))
{
mapper_->setParamCorrelationSearchSpaceSmearDeviation(
correlation_search_space_smear_deviation);
}
// Setting Correlation Parameters, Loop Closure Parameters
double loop_search_space_dimension;
if(nh.getParam("loop_search_space_dimension", loop_search_space_dimension))
{
mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);
}
double loop_search_space_resolution;
if(nh.getParam("loop_search_space_resolution", loop_search_space_resolution))
{
mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);
}
double loop_search_space_smear_deviation;
if(nh.getParam("loop_search_space_smear_deviation",
loop_search_space_smear_deviation))
{
mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);
}
// Setting Scan Matcher Parameters
double distance_variance_penalty;
if(nh.getParam("distance_variance_penalty", distance_variance_penalty))
{
mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);
}
double angle_variance_penalty;
if(nh.getParam("angle_variance_penalty", angle_variance_penalty))
{
mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
}
double fine_search_angle_offset;
if(nh.getParam("fine_search_angle_offset", fine_search_angle_offset))
{
mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
}
double coarse_search_angle_offset;
if(nh.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
{
mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);
}
double coarse_angle_resolution;
if(nh.getParam("coarse_angle_resolution", coarse_angle_resolution))
{
mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);
}
double minimum_angle_penalty;
if(nh.getParam("minimum_angle_penalty", minimum_angle_penalty))
{
mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);
}
double minimum_distance_penalty;
if(nh.getParam("minimum_distance_penalty", minimum_distance_penalty))
{
mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);
}
bool use_response_expansion;
if(nh.getParam("use_response_expansion", use_response_expansion))
{
mapper_->setParamUseResponseExpansion(use_response_expansion);
}
return;
}
/*****************************************************************************/
void SMapper::Reset()
/*****************************************************************************/
{
mapper_->Reset();
return;
}
} // end namespace

75
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async.cpp Normal file
View File

@@ -0,0 +1,75 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_async.hpp"
namespace slam_toolbox
{
/*****************************************************************************/
AsynchronousSlamToolbox::AsynchronousSlamToolbox(ros::NodeHandle& nh)
: SlamToolbox(nh)
/*****************************************************************************/
{
loadPoseGraphByParams(nh);
}
/*****************************************************************************/
void AsynchronousSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
karto::Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
{
return;
}
// ensure the laser can be used
karto::LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN_THROTTLE(5., "Failed to create laser device for"
" %s; discarding scan", scan->header.frame_id.c_str());
return;
}
addScan(laser, scan, pose);
return;
}
/*****************************************************************************/
bool AsynchronousSlamToolbox::deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
/*****************************************************************************/
{
if (req.match_type == procType::LOCALIZE_AT_POSE)
{
ROS_ERROR("Requested a localization deserialization "
"in non-localization mode.");
return false;
}
return SlamToolbox::deserializePoseGraphCallback(req, resp);
}
} // end namespace

46
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async_node.cpp Normal file
View File

@@ -0,0 +1,46 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_async.hpp"
int main(int argc, char** argv)
{
ros::init(argc, argv, "slam_toolbox");
ros::NodeHandle nh("~");
ros::spinOnce();
int stack_size;
if (nh.getParam("stack_size_to_use", stack_size))
{
ROS_INFO("Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
getrlimit(RLIMIT_STACK, &stack_limit);
if (stack_limit.rlim_cur < stack_size)
{
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
}
slam_toolbox::AsynchronousSlamToolbox sst(nh);
ros::spin();
return 0;
}

850
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_common.cpp Normal file
View File

@@ -0,0 +1,850 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Samsung Research America
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Orginal Author for slam_karto the original work was based on: Brian Gerkey */
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_common.hpp"
#include "slam_toolbox/serialization.hpp"
namespace slam_toolbox
{
/*****************************************************************************/
SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
: solver_loader_("slam_toolbox", "karto::ScanSolver"),
processor_type_(PROCESS),
first_measurement_(true),
nh_(nh),
process_near_pose_(nullptr),
using_loop_(false),
initialized_(false)
/*****************************************************************************/
{
if (!initialized_)
{
smapper_ = std::make_unique<mapper_utils::SMapper>();
dataset_ = std::make_unique<karto::Dataset>();
setParams(nh_);
setROSInterfaces(nh_);
setSolver(nh_);
laser_assistant_ = std::make_unique<laser_utils::LaserAssistant>(
nh_, tf_.get(), base_frame_);
pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
tf_.get(), base_frame_, odom_frame_);
scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
closure_assistant_ =
std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_);
reprocessing_transform_.setIdentity();
double transform_publish_period;
nh_.param("transform_publish_period", transform_publish_period, 0.05);
using_loop_ = true;
threads_.push_back(std::make_unique<boost::thread>(
boost::bind(&SlamToolbox::publishTransformLoop,
this, transform_publish_period)));
threads_.push_back(std::make_unique<boost::thread>(
boost::bind(&SlamToolbox::publishVisualizations, this)));
bool all_joinable = false;
while (ros::ok() && !all_joinable)
{
all_joinable = true;
for (auto& p : threads_)
{
if (!p->joinable())
{
all_joinable = false;
break;
}
}
}
ROS_INFO("All threads are joinable, proceeding with next steps.");
initialized_ = true;
}
}
/*****************************************************************************/
SlamToolbox::~SlamToolbox()
/*****************************************************************************/
{
using_loop_ = false;
for (int i = 0; i != threads_.size(); i++)
{
threads_[i]->join();
}
smapper_.reset();
dataset_.reset();
closure_assistant_.reset();
map_saver_.reset();
pose_helper_.reset();
laser_assistant_.reset();
scan_holder_.reset();
}
/*****************************************************************************/
void SlamToolbox::setSolver(ros::NodeHandle& private_nh_)
/*****************************************************************************/
{
// Set solver to be used in loop closure
std::string solver_plugin;
if (!private_nh_.getParam("solver_plugin", solver_plugin))
{
ROS_WARN("unable to find requested solver plugin, defaulting to SPA");
solver_plugin = "solver_plugins::CeresSolver";
}
try
{
solver_ = solver_loader_.createInstance(solver_plugin);
ROS_INFO("Using plugin %s", solver_plugin.c_str());
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.",
solver_plugin.c_str(), ex.what());
exit(1);
}
smapper_->getMapper()->SetScanSolver(solver_.get());
}
/*****************************************************************************/
void SlamToolbox::setParams(ros::NodeHandle& private_nh)
/*****************************************************************************/
{
map_to_odom_.setIdentity();
private_nh.param("odom_frame", odom_frame_, std::string("odom"));
private_nh.param("map_frame", map_frame_, std::string("map"));
private_nh.param("base_frame", base_frame_, std::string("base_footprint"));
private_nh.param("resolution", resolution_, 0.05);
private_nh.param("map_name", map_name_, std::string("/map"));
private_nh.param("scan_topic", scan_topic_, std::string("/scan"));
private_nh.param("throttle_scans", throttle_scans_, 1);
private_nh.param("enable_interactive_mode", enable_interactive_mode_, false);
double tmp_val;
private_nh.param("transform_timeout", tmp_val, 0.2);
transform_timeout_ = ros::Duration(tmp_val);
private_nh.param("tf_buffer_duration", tmp_val, 30.);
tf_buffer_dur_ = ros::Duration(tmp_val);
private_nh.param("minimum_time_interval", tmp_val, 0.5);
minimum_time_interval_ = ros::Duration(tmp_val);
private_nh.param("position_covariance_scale", position_covariance_scale_, 1.0);
private_nh.param("yaw_covariance_scale", yaw_covariance_scale_, 1.0);
bool debug = false;
if (private_nh.getParam("debug_logging", debug) && debug)
{
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
ros::console::levels::Debug))
{
ros::console::notifyLoggerLevelsChanged();
}
}
smapper_->configure(private_nh);
private_nh.setParam("paused_new_measurements", false);
}
/*****************************************************************************/
void SlamToolbox::setROSInterfaces(ros::NodeHandle& node)
/*****************************************************************************/
{
tf_ = std::make_unique<tf2_ros::Buffer>(ros::Duration(tf_buffer_dur_));
tfL_ = std::make_unique<tf2_ros::TransformListener>(*tf_);
tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
sst_ = node.advertise<nav_msgs::OccupancyGrid>(map_name_, 1, true);
sstm_ = node.advertise<nav_msgs::MapMetaData>(map_name_ + "_metadata", 1, true);
ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this);
ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this);
ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this);
ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this);
ssReset_ = node.advertiseService("reset", &SlamToolbox::resetCallback, this);
scan_filter_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_sub_, *tf_, odom_frame_, 5, node);
scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
pose_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10, true);
}
/*****************************************************************************/
void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
/*****************************************************************************/
{
if (transform_publish_period == 0)
{
return;
}
ros::Rate r(1.0 / transform_publish_period);
while (ros::ok() && using_loop_)
{
{
boost::mutex::scoped_lock lock(map_to_odom_mutex_);
geometry_msgs::TransformStamped msg;
tf2::convert(map_to_odom_, msg.transform);
msg.child_frame_id = odom_frame_;
msg.header.frame_id = map_frame_;
msg.header.stamp = ros::Time::now() + transform_timeout_;
tfB_->sendTransform(msg);
}
r.sleep();
}
}
/*****************************************************************************/
void SlamToolbox::publishVisualizations()
/*****************************************************************************/
{
nav_msgs::OccupancyGrid& og = map_.map;
og.info.resolution = resolution_;
og.info.origin.position.x = 0.0;
og.info.origin.position.y = 0.0;
og.info.origin.position.z = 0.0;
og.info.origin.orientation.x = 0.0;
og.info.origin.orientation.y = 0.0;
og.info.origin.orientation.z = 0.0;
og.info.origin.orientation.w = 1.0;
og.header.frame_id = map_frame_;
double map_update_interval;
if (!nh_.getParam("map_update_interval", map_update_interval))
map_update_interval = 10.0;
ros::Rate r(1.0 / map_update_interval);
while (ros::ok() && using_loop_)
{
this->updateMap();
if (!isPaused(VISUALIZING_GRAPH))
{
boost::mutex::scoped_lock lock(smapper_mutex_);
closure_assistant_->publishGraph();
}
r.sleep();
}
}
/*****************************************************************************/
void SlamToolbox::loadPoseGraphByParams(ros::NodeHandle& nh)
/*****************************************************************************/
{
std::string filename;
geometry_msgs::Pose2D pose;
bool dock = false;
if (shouldStartWithPoseGraph(filename, pose, dock))
{
slam_toolbox_msgs::DeserializePoseGraph::Request req;
slam_toolbox_msgs::DeserializePoseGraph::Response resp;
req.initial_pose = pose;
req.filename = filename;
if (dock)
{
req.match_type =
slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE;
}
else
{
req.match_type =
slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE;
}
deserializePoseGraphCallback(req, resp);
}
}
/*****************************************************************************/
bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename,
geometry_msgs::Pose2D& pose, bool& start_at_dock)
/*****************************************************************************/
{
// if given a map to load at run time, do it.
if (nh_.getParam("map_file_name", filename))
{
std::vector<double> read_pose;
if (nh_.getParam("map_start_pose", read_pose))
{
start_at_dock = false;
if (read_pose.size() != 3)
{
ROS_ERROR("LocalizationSlamToolbox: Incorrect number of "
"arguments for map starting pose. Must be in format: "
"[x, y, theta]. Starting at the origin");
pose.x = 0.;
pose.y = 0.;
pose.theta = 0.;
}
else
{
pose.x = read_pose[0];
pose.y = read_pose[1];
pose.theta = read_pose[2];
}
}
else
{
nh_.getParam("map_start_at_dock", start_at_dock);
}
return true;
}
return false;
}
/*****************************************************************************/
karto::LaserRangeFinder* SlamToolbox::getLaser(const
sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
const std::string& frame = scan->header.frame_id;
if (lasers_.find(frame) == lasers_.end())
{
try
{
lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
dataset_->Add(lasers_[frame].getLaser(), true);
}
catch (tf2::TransformException& e)
{
ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return nullptr;
}
}
return lasers_[frame].getLaser();
}
/*****************************************************************************/
bool SlamToolbox::updateMap()
/*****************************************************************************/
{
if (sst_.getNumSubscribers() == 0)
{
return true;
}
boost::mutex::scoped_lock lock(smapper_mutex_);
karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_);
if (!occ_grid)
{
return false;
}
vis_utils::toNavMap(occ_grid, map_.map);
// publish map as current
map_.map.header.stamp = ros::Time::now();
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
delete occ_grid;
occ_grid = nullptr;
return true;
}
/*****************************************************************************/
tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
const karto::Pose2& corrected_pose,
const karto::Pose2& karto_pose,
const ros::Time& t,
const bool& update_reprocessing_transform)
/*****************************************************************************/
{
// Compute the map->odom transform
tf2::Stamped<tf2::Transform> odom_to_map;
tf2::Quaternion q(0., 0., 0., 1.0);
q.setRPY(0., 0., corrected_pose.GetHeading());
tf2::Stamped<tf2::Transform> base_to_map(
tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_);
try
{
geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
tf2::convert(base_to_map, base_to_map_msg);
odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_);
tf2::convert(odom_to_map_msg, odom_to_map);
}
catch (tf2::TransformException& e)
{
ROS_ERROR("Transform from base_link to odom failed: %s", e.what());
return odom_to_map;
}
// if we're continuing a previous session, we need to
// estimate the homogenous transformation between the old and new
// odometry frames and transform the new session
// into the older session's frame
if (update_reprocessing_transform)
{
tf2::Transform odom_to_base_serialized = base_to_map.inverse();
tf2::Quaternion q1(0., 0., 0., 1.0);
q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation()));
odom_to_base_serialized.setRotation(q1);
tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose);
reprocessing_transform_ =
odom_to_base_serialized * odom_to_base_current.inverse();
}
// set map to odom for our transformation thread to publish
boost::mutex::scoped_lock lock(map_to_odom_mutex_);
map_to_odom_ = tf2::Transform(tf2::Quaternion(odom_to_map.getRotation()),
tf2::Vector3(odom_to_map.getOrigin())).inverse();
return odom_to_map;
}
/*****************************************************************************/
karto::LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan(
karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
/*****************************************************************************/
{
// Create a vector of doubles for karto
std::vector<kt_double> readings = laser_utils::scanToReadings(
*scan, lasers_[scan->header.frame_id].isInverted());
// transform by the reprocessing transform
tf2::Transform pose_original = smapper_->toTfPose(karto_pose);
tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original;
karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed);
// create localized range scan
karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan(
laser->GetName(), readings);
range_scan->SetOdometricPose(transformed_pose);
range_scan->SetCorrectedPose(transformed_pose);
return range_scan;
}
/*****************************************************************************/
bool SlamToolbox::shouldProcessScan(
const sensor_msgs::LaserScan::ConstPtr& scan,
const karto::Pose2& pose)
/*****************************************************************************/
{
static karto::Pose2 last_pose;
static ros::Time last_scan_time = ros::Time(0.);
static double min_dist2 =
smapper_->getMapper()->getParamMinimumTravelDistance() *
smapper_->getMapper()->getParamMinimumTravelDistance();
// we give it a pass on the first measurement to get the ball rolling
if (first_measurement_)
{
last_scan_time = scan->header.stamp;
last_pose = pose;
first_measurement_ = false;
return true;
}
// we are in a paused mode, reject incomming information
if (isPaused(NEW_MEASUREMENTS))
{
return false;
}
// throttled out
if ((scan->header.seq % throttle_scans_) != 0)
{
return false;
}
// not enough time
if (scan->header.stamp - last_scan_time < minimum_time_interval_)
{
return false;
}
// check moved enough, within 10% for correction error
const double dist2 = last_pose.SquaredDistance(pose);
if (dist2 < 0.8 * min_dist2 || scan->header.seq < 5)
{
return false;
}
last_pose = pose;
last_scan_time = scan->header.stamp;
return true;
}
/*****************************************************************************/
karto::LocalizedRangeScan* SlamToolbox::addScan(
karto::LaserRangeFinder* laser,
PosedScan& scan_w_pose)
/*****************************************************************************/
{
return addScan(laser, scan_w_pose.scan, scan_w_pose.pose);
}
/*****************************************************************************/
karto::LocalizedRangeScan* SlamToolbox::addScan(
karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
/*****************************************************************************/
{
// get our localized range scan
karto::LocalizedRangeScan* range_scan = getLocalizedRangeScan(
laser, scan, karto_pose);
// Add the localized range scan to the smapper
boost::mutex::scoped_lock lock(smapper_mutex_);
bool processed = false, update_reprocessing_transform = false;
karto::Matrix3 covariance;
covariance.SetToIdentity();
if (processor_type_ == PROCESS)
{
processed = smapper_->getMapper()->Process(range_scan, &covariance);
}
else if (processor_type_ == PROCESS_FIRST_NODE)
{
processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
processor_type_ = PROCESS;
update_reprocessing_transform = true;
}
else if (processor_type_ == PROCESS_NEAR_REGION)
{
boost::mutex::scoped_lock l(pose_mutex_);
if (!process_near_pose_)
{
ROS_ERROR("Process near region called without a "
"valid region request. Ignoring scan.");
return nullptr;
}
range_scan->SetOdometricPose(*process_near_pose_);
range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
process_near_pose_.reset(nullptr);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, false, &covariance);
update_reprocessing_transform = true;
processor_type_ = PROCESS;
}
else
{
ROS_FATAL("SlamToolbox: No valid processor type set! Exiting.");
exit(-1);
}
// if successfully processed, create odom to map transformation
// and add our scan to storage
if (processed)
{
if (enable_interactive_mode_)
{
scan_holder_->addScan(*scan);
}
setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
scan->header.stamp, update_reprocessing_transform);
dataset_->Add(range_scan);
publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
}
else
{
delete range_scan;
range_scan = nullptr;
}
return range_scan;
}
/*****************************************************************************/
void SlamToolbox::publishPose(
const karto::Pose2& pose,
const karto::Matrix3& cov,
const ros::Time& t)
/*****************************************************************************/
{
geometry_msgs::PoseWithCovarianceStamped pose_msg;
pose_msg.header.stamp = t;
pose_msg.header.frame_id = map_frame_;
tf2::Quaternion q(0., 0., 0., 1.0);
q.setRPY(0., 0., pose.GetHeading());
tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0));
tf2::toMsg(transform, pose_msg.pose.pose);
pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x
pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy
pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy
pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y
pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw
pose_pub_.publish(pose_msg);
}
/*****************************************************************************/
bool SlamToolbox::mapCallback(
nav_msgs::GetMap::Request& req,
nav_msgs::GetMap::Response& res)
/*****************************************************************************/
{
if (map_.map.info.width && map_.map.info.height)
{
boost::mutex::scoped_lock lock(smapper_mutex_);
res = map_;
return true;
}
else
{
return false;
}
}
/*****************************************************************************/
bool SlamToolbox::pauseNewMeasurementsCallback(
slam_toolbox_msgs::Pause::Request& req,
slam_toolbox_msgs::Pause::Response& resp)
/*****************************************************************************/
{
bool curr_state = isPaused(NEW_MEASUREMENTS);
state_.set(NEW_MEASUREMENTS, !curr_state);
nh_.setParam("paused_new_measurements", !curr_state);
ROS_INFO("SlamToolbox: Toggled to %s",
!curr_state ? "pause taking new measurements." :
"actively taking new measurements.");
resp.status = true;
return true;
}
/*****************************************************************************/
bool SlamToolbox::isPaused(const PausedApplication& app)
/*****************************************************************************/
{
return state_.get(app);
}
/*****************************************************************************/
bool SlamToolbox::serializePoseGraphCallback(
slam_toolbox_msgs::SerializePoseGraph::Request& req,
slam_toolbox_msgs::SerializePoseGraph::Response& resp)
/*****************************************************************************/
{
std::string filename = req.filename;
// if we're inside the snap, we need to write to commonly accessible space
if (snap_utils::isInSnap())
{
filename = snap_utils::getSnapPath() + std::string("/") + filename;
}
boost::mutex::scoped_lock lock(smapper_mutex_);
serialization::write(filename, *smapper_->getMapper(), *dataset_);
return true;
}
/*****************************************************************************/
void SlamToolbox::loadSerializedPoseGraph(
std::unique_ptr<karto::Mapper>& mapper,
std::unique_ptr<karto::Dataset>& dataset)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(smapper_mutex_);
solver_->Reset();
// add the nodes and constraints to the optimizer
VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices();
VerticeMap::iterator vertex_map_it = mapper_vertices.begin();
for (vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it)
{
ScanMap::iterator vertex_it = vertex_map_it->second.begin();
for (vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it)
{
if (vertex_it->second != nullptr)
{
solver_->AddNode(vertex_it->second);
}
}
}
EdgeVector mapper_edges = mapper->GetGraph()->GetEdges();
EdgeVector::iterator edges_it = mapper_edges.begin();
for (edges_it; edges_it != mapper_edges.end(); ++edges_it)
{
if (*edges_it != nullptr)
{
solver_->AddConstraint(*edges_it);
}
}
mapper->SetScanSolver(solver_.get());
// move the memory to our working dataset
smapper_->setMapper(mapper.release());
smapper_->configure(nh_);
dataset_.reset(dataset.release());
closure_assistant_->setMapper(smapper_->getMapper());
if (!smapper_->getMapper())
{
ROS_FATAL("loadSerializedPoseGraph: Could not properly load "
"a valid mapping object. Did you modify something by hand?");
exit(-1);
}
if (dataset_->GetLasers().size() < 1)
{
ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize "
"dataset with no laser objects.");
exit(-1);
}
// create a current laser sensor
karto::LaserRangeFinder* laser =
dynamic_cast<karto::LaserRangeFinder*>(
dataset_->GetLasers()[0]);
karto::Sensor* pSensor = dynamic_cast<karto::Sensor*>(laser);
if (pSensor)
{
karto::SensorManager::GetInstance()->RegisterSensor(pSensor);
while (ros::ok())
{
ROS_INFO("Waiting for incoming scan to get metadata...");
boost::shared_ptr<sensor_msgs::LaserScan const> scan =
ros::topic::waitForMessage<sensor_msgs::LaserScan>(
scan_topic_, ros::Duration(1.0));
if (scan)
{
ROS_INFO("Got scan!");
try
{
lasers_[scan->header.frame_id] =
laser_assistant_->toLaserMetadata(*scan);
break;
}
catch (tf2::TransformException& e)
{
ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)",
e.what());
exit(-1);
}
}
}
}
else
{
ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor.");
}
solver_->Compute();
return;
}
/*****************************************************************************/
bool SlamToolbox::deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
/*****************************************************************************/
{
if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET)
{
ROS_ERROR("Deserialization called without valid processor type set. "
"Undefined behavior!");
return false;
}
std::string filename = req.filename;
if (filename.empty())
{
ROS_WARN("No map file given!");
return true;
}
// if we're inside the snap, we need to write to commonly accessible space
if (snap_utils::isInSnap())
{
filename = snap_utils::getSnapPath() + std::string("/") + filename;
}
std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
if (!serialization::read(filename, *mapper, *dataset))
{
ROS_ERROR("DeserializePoseGraph: Failed to read "
"file: %s.", filename.c_str());
return true;
}
ROS_DEBUG("DeserializePoseGraph: Successfully read file.");
loadSerializedPoseGraph(mapper, dataset);
updateMap();
first_measurement_ = true;
boost::mutex::scoped_lock l(pose_mutex_);
switch (req.match_type)
{
case procType::START_AT_FIRST_NODE:
processor_type_ = PROCESS_FIRST_NODE;
break;
case procType::START_AT_GIVEN_POSE:
processor_type_ = PROCESS_NEAR_REGION;
process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
req.initial_pose.y, req.initial_pose.theta);
break;
case procType::LOCALIZE_AT_POSE:
processor_type_ = PROCESS_LOCALIZATION;
process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
req.initial_pose.y, req.initial_pose.theta);
break;
default:
ROS_FATAL("Deserialization called without valid processor type set.");
}
return true;
}
/*****************************************************************************/
bool SlamToolbox::resetCallback(
slam_toolbox_msgs::Reset::Request& req,
slam_toolbox_msgs::Reset::Response& resp)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(smapper_mutex_);
// Reset the map.
smapper_->Reset();
smapper_->getMapper()->getScanSolver()->Reset();
// Ensure we will process the next available scan.
first_measurement_ = true;
// Pause new measurements processing if requested.
if (req.pause_new_measurements) {
state_.set(NEW_MEASUREMENTS, true);
nh_.setParam("paused_new_measurements", true);
ROS_INFO("SlamToolbox: Toggled to pause taking new measurements after reset.");
}
resp.result = true;
return true;
}
} // end namespace

232
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization.cpp Normal file
View File

@@ -0,0 +1,232 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_localization.hpp"
namespace slam_toolbox
{
/*****************************************************************************/
LocalizationSlamToolbox::LocalizationSlamToolbox(ros::NodeHandle& nh)
: SlamToolbox(nh)
/*****************************************************************************/
{
processor_type_ = PROCESS_LOCALIZATION;
localization_pose_sub_ = nh.subscribe("/initialpose", 1,
&LocalizationSlamToolbox::localizePoseCallback, this);
clear_localization_ = nh.advertiseService(
"clear_localization_buffer",
&LocalizationSlamToolbox::clearLocalizationBuffer, this);
std::string filename;
geometry_msgs::Pose2D pose;
bool dock = false;
if (shouldStartWithPoseGraph(filename, pose, dock))
{
slam_toolbox_msgs::DeserializePoseGraph::Request req;
slam_toolbox_msgs::DeserializePoseGraph::Response resp;
req.initial_pose = pose;
req.filename = filename;
req.match_type =
slam_toolbox_msgs::DeserializePoseGraph::Request::LOCALIZE_AT_POSE;
if (dock)
{
ROS_ERROR("LocalizationSlamToolbox: Starting localization "
"at first node (dock) is correctly not supported.");
}
deserializePoseGraphCallback(req, resp);
}
// in localization mode, we cannot allow for interactive mode
enable_interactive_mode_ = false;
// in localization mode, disable map saver
map_saver_.reset();
return;
}
/*****************************************************************************/
bool LocalizationSlamToolbox::clearLocalizationBuffer(
std_srvs::Empty::Request& req,
std_srvs::Empty::Response& resp)
/*****************************************************************************/
{
boost::mutex::scoped_lock lock(smapper_mutex_);
ROS_INFO("LocalizationSlamToolbox: Clearing localization buffer.");
smapper_->clearLocalizationBuffer();
return true;
}
/*****************************************************************************/
bool LocalizationSlamToolbox::serializePoseGraphCallback(
slam_toolbox_msgs::SerializePoseGraph::Request& req,
slam_toolbox_msgs::SerializePoseGraph::Response& resp)
/*****************************************************************************/
{
ROS_FATAL("LocalizationSlamToolbox: Cannot call serialize map "
"in localization mode!");
return false;
}
/*****************************************************************************/
bool LocalizationSlamToolbox::deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
/*****************************************************************************/
{
if (req.match_type != procType::LOCALIZE_AT_POSE)
{
ROS_ERROR("Requested a non-localization deserialization "
"in localization mode.");
return false;
}
return SlamToolbox::deserializePoseGraphCallback(req, resp);
}
/*****************************************************************************/
void LocalizationSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
{
return;
}
// ensure the laser can be used
LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser"
" device for %s; discarding scan", scan->header.frame_id.c_str());
return;
}
if (shouldProcessScan(scan, pose))
{
addScan(laser, scan, pose);
}
return;
}
/*****************************************************************************/
LocalizedRangeScan* LocalizationSlamToolbox::addScan(
LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
Pose2& karto_pose)
/*****************************************************************************/
{
boost::mutex::scoped_lock l(pose_mutex_);
if (PROCESS_LOCALIZATION && process_near_pose_)
{
processor_type_ = PROCESS_NEAR_REGION;
}
LocalizedRangeScan* range_scan = getLocalizedRangeScan(
laser, scan, karto_pose);
// Add the localized range scan to the smapper
boost::mutex::scoped_lock lock(smapper_mutex_);
bool processed = false, update_reprocessing_transform = false;
if (processor_type_ == PROCESS_NEAR_REGION)
{
if (!process_near_pose_)
{
ROS_ERROR("Process near region called without a "
"valid region request. Ignoring scan.");
return nullptr;
}
// set our position to the requested pose and process
range_scan->SetOdometricPose(*process_near_pose_);
range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
process_near_pose_.reset(nullptr);
processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true);
// reset to localization mode
processor_type_ = PROCESS_LOCALIZATION;
update_reprocessing_transform = true;
}
else if (processor_type_ == PROCESS_LOCALIZATION)
{
processed = smapper_->getMapper()->ProcessLocalization(range_scan);
update_reprocessing_transform = false;
}
else
{
ROS_FATAL("LocalizationSlamToolbox: "
"No valid processor type set! Exiting.");
exit(-1);
}
// if successfully processed, create odom to map transformation
if(!processed)
{
delete range_scan;
range_scan = nullptr;
} else {
// compute our new transform
setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
scan->header.stamp, update_reprocessing_transform);
}
return range_scan;
}
/*****************************************************************************/
void LocalizationSlamToolbox::localizePoseCallback(const
geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
/*****************************************************************************/
{
if (processor_type_ != PROCESS_LOCALIZATION)
{
ROS_ERROR("LocalizePoseCallback: Cannot process localization command "
"if not in localization mode.");
return;
}
boost::mutex::scoped_lock l(pose_mutex_);
if (process_near_pose_)
{
process_near_pose_.reset(new Pose2(msg->pose.pose.position.x,
msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation)));
}
else
{
process_near_pose_ = std::make_unique<Pose2>(msg->pose.pose.position.x,
msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation));
}
first_measurement_ = true;
boost::mutex::scoped_lock lock(smapper_mutex_);
smapper_->clearLocalizationBuffer();
ROS_INFO("LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f",
msg->pose.pose.position.x, msg->pose.pose.position.y,
tf2::getYaw(msg->pose.pose.orientation));
return;
}
} // end namespace

45
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization_node.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_localization.hpp"
int main(int argc, char** argv)
{
ros::init(argc, argv, "slam_toolbox");
ros::NodeHandle nh("~");
ros::spinOnce();
int stack_size;
if (nh.getParam("stack_size_to_use", stack_size))
{
ROS_INFO("Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
getrlimit(RLIMIT_STACK, &stack_limit);
if (stack_limit.rlim_cur < stack_size)
{
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
}
slam_toolbox::LocalizationSlamToolbox sst(nh);
ros::spin();
return 0;
}

154
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync.cpp Normal file
View File

@@ -0,0 +1,154 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_sync.hpp"
namespace slam_toolbox
{
/*****************************************************************************/
SynchronousSlamToolbox::SynchronousSlamToolbox(ros::NodeHandle& nh)
: SlamToolbox(nh)
/*****************************************************************************/
{
ssClear_ = nh.advertiseService("clear_queue",
&SynchronousSlamToolbox::clearQueueCallback, this);
threads_.push_back(std::make_unique<boost::thread>(
boost::bind(&SynchronousSlamToolbox::run, this)));
loadPoseGraphByParams(nh);
}
/*****************************************************************************/
void SynchronousSlamToolbox::run()
/*****************************************************************************/
{
ros::Rate r(100);
while(ros::ok())
{
if (!isPaused(PROCESSING))
{
PosedScan scan_w_pose(nullptr, karto::Pose2()); // dummy, updated in critical section
bool queue_empty = true;
{
boost::mutex::scoped_lock lock(q_mutex_);
queue_empty = q_.empty();
if(!queue_empty)
{
scan_w_pose = q_.front();
q_.pop();
if (q_.size() > 10)
{
ROS_WARN_THROTTLE(10., "Queue size has grown to: %i. "
"Recommend stopping until message is gone if online mapping.",
(int)q_.size());
}
}
}
if(!queue_empty){
addScan(getLaser(scan_w_pose.scan), scan_w_pose);
continue;
}
}
r.sleep();
}
}
/*****************************************************************************/
void SynchronousSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
karto::Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
{
return;
}
// ensure the laser can be used
karto::LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser"
" device for %s; discarding scan", scan->header.frame_id.c_str());
return;
}
// if sync and valid, add to queue
if (shouldProcessScan(scan, pose))
{
boost::mutex::scoped_lock lock(q_mutex_);
q_.push(PosedScan(scan, pose));
}
return;
}
/*****************************************************************************/
bool SynchronousSlamToolbox::clearQueueCallback(
slam_toolbox_msgs::ClearQueue::Request& req,
slam_toolbox_msgs::ClearQueue::Response& resp)
/*****************************************************************************/
{
ROS_INFO("SynchronousSlamToolbox: Clearing all queued scans to add to map.");
while(!q_.empty())
{
q_.pop();
}
resp.status = true;
return true;
}
/*****************************************************************************/
bool SynchronousSlamToolbox::deserializePoseGraphCallback(
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
/*****************************************************************************/
{
if (req.match_type == procType::LOCALIZE_AT_POSE)
{
ROS_ERROR("Requested a localization deserialization "
"in non-localization mode.");
return false;
}
return SlamToolbox::deserializePoseGraphCallback(req, resp);
}
/*****************************************************************************/
bool SynchronousSlamToolbox::resetCallback(
slam_toolbox_msgs::Reset::Request& req,
slam_toolbox_msgs::Reset::Response& resp)
/*****************************************************************************/
{
{
boost::mutex::scoped_lock lock(q_mutex_);
// Clear the scan queue.
while (!q_.empty()) {
q_.pop();
}
}
return SlamToolbox::resetCallback(req, resp);
}
} // end namespace

46
Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync_node.cpp Normal file
View File

@@ -0,0 +1,46 @@
/*
* slam_toolbox
* Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
* Copyright Work Modifications (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#include "slam_toolbox/slam_toolbox_sync.hpp"
int main(int argc, char** argv)
{
ros::init(argc, argv, "slam_toolbox");
ros::NodeHandle nh("~");
ros::spinOnce();
int stack_size;
if (nh.getParam("stack_size_to_use", stack_size))
{
ROS_INFO("Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
getrlimit(RLIMIT_STACK, &stack_limit);
if (stack_limit.rlim_cur < stack_size)
{
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
}
slam_toolbox::SynchronousSlamToolbox sst(nh);
ros::spin();
return 0;
}

8841
Localizations/Packages/slam_toolbox/slam_toolbox/test/constraints_on_graph.dat Normal file
View File

File diff suppressed because it is too large Load Diff

187
Localizations/Packages/slam_toolbox/slam_toolbox/test/lifelong_metrics_test.cpp Normal file
View File

@@ -0,0 +1,187 @@
/*
* slam_toolbox
* Copyright (c) 2019, Steve Macenski
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Steven Macenski */
#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_
#define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_
#include <gtest/gtest.h>
#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp"
using namespace karto;
// 3 potential test cases, t1 is used.
//t1 = IOU([3.5, 4.0, 3.0, 4.0], [3.5, 5.5, 3.0, 3.0]) == 6.0
//t2 = IOU([4.5, 3.0, 5.0, 2.0], [4.5, 4.5, 3, 3]) == 3.0
//t3 = IOU([4.5, 3.5, 3.0, 3.0], [2.5, 5.5, 3, 3]) == 1.0
namespace
{
TEST(LifelingMetricsTests, TestBounds)
{
LocalizedRangeScan* s1 = new LocalizedRangeScan();
LocalizedRangeScan* s2 = new LocalizedRangeScan();
Pose2 p1 = Pose2(3.5, 4.0, 0.0);
Pose2 p2 = Pose2(3.5, 5.5, 0.0);
s1->SetBarycenterPose(p1);
s2->SetBarycenterPose(p2);
BoundingBox2 bb1, bb2;
bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
s1->SetBoundingBox(bb1);
s2->SetBoundingBox(bb2);
PointVectorDouble pts;
pts.push_back(Vector2<double>(3.0, 5.0)); //inside
pts.push_back(Vector2<double>(3.0, 3.0)); //outside
s2->SetPointReadings(pts, true);
double x_l, x_u, y_l, y_u;
bool dirty = false;
s1->SetIsDirty(dirty);
s2->SetIsDirty(dirty);
slam_toolbox::LifelongSlamToolbox::computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u);
EXPECT_EQ(x_l, 2.0);
EXPECT_EQ(x_u, 5.0);
EXPECT_EQ(y_l, 4.0);
EXPECT_EQ(y_u, 6.0);
delete s1;
delete s2;
}
TEST(LifelingMetricsTests, TestIntersect)
{
LocalizedRangeScan* s1 = new LocalizedRangeScan();
LocalizedRangeScan* s2 = new LocalizedRangeScan();
Pose2 p1 = Pose2(3.5, 4.0, 0.0);
Pose2 p2 = Pose2(3.5, 5.5, 0.0);
s1->SetBarycenterPose(p1);
s2->SetBarycenterPose(p2);
BoundingBox2 bb1, bb2;
bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
s1->SetBoundingBox(bb1);
s2->SetBoundingBox(bb2);
PointVectorDouble pts;
pts.push_back(Vector2<double>(3.0, 5.0)); //inside
pts.push_back(Vector2<double>(3.0, 3.0)); //outside
s2->SetPointReadings(pts, true);
bool dirty = false;
s1->SetIsDirty(dirty);
s2->SetIsDirty(dirty);
double intersect = slam_toolbox::LifelongSlamToolbox::computeIntersect(s1, s2);
EXPECT_EQ(intersect, 6.0);
delete s1;
delete s2;
}
TEST(LifelingMetricsTests, TestIntersectOverUnion)
{
LocalizedRangeScan* s1 = new LocalizedRangeScan();
LocalizedRangeScan* s2 = new LocalizedRangeScan();
Pose2 p1 = Pose2(3.5, 4.0, 0.0);
Pose2 p2 = Pose2(3.5, 5.5, 0.0);
s1->SetBarycenterPose(p1);
s2->SetBarycenterPose(p2);
BoundingBox2 bb1, bb2;
bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
s1->SetBoundingBox(bb1);
s2->SetBoundingBox(bb2);
PointVectorDouble pts;
pts.push_back(Vector2<double>(3.0, 5.0)); //inside
pts.push_back(Vector2<double>(3.0, 3.0)); //outside
s2->SetPointReadings(pts, true);
bool dirty = false;
s1->SetIsDirty(dirty);
s2->SetIsDirty(dirty);
double intersect_over_union = slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion(s1, s2);
EXPECT_EQ(intersect_over_union, 0.4);
delete s1;
delete s2;
}
TEST(LifelingMetricsTests, TestAreaOverlap)
{
LocalizedRangeScan* s1 = new LocalizedRangeScan();
LocalizedRangeScan* s2 = new LocalizedRangeScan();
Pose2 p1 = Pose2(3.5, 4.0, 0.0);
Pose2 p2 = Pose2(3.5, 5.5, 0.0);
s1->SetBarycenterPose(p1);
s2->SetBarycenterPose(p2);
BoundingBox2 bb1, bb2;
bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
s1->SetBoundingBox(bb1);
s2->SetBoundingBox(bb2);
PointVectorDouble pts;
pts.push_back(Vector2<double>(3.0, 5.0)); //inside
pts.push_back(Vector2<double>(3.0, 3.0)); //outside
s2->SetPointReadings(pts, true);
bool dirty = false;
s1->SetIsDirty(dirty);
s2->SetIsDirty(dirty);
double area = slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio(s1, s2);
EXPECT_NEAR(area, 0.6666, 0.01);
delete s1;
delete s2;
}
TEST(LifelingMetricsTests, TestPtOverlap)
{
LocalizedRangeScan* s1 = new LocalizedRangeScan();
LocalizedRangeScan* s2 = new LocalizedRangeScan();
Pose2 p1 = Pose2(3.5, 4.0, 0.0);
Pose2 p2 = Pose2(3.5, 5.5, 0.0);
s1->SetBarycenterPose(p1);
s2->SetBarycenterPose(p2);
BoundingBox2 bb1, bb2;
bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
s1->SetBoundingBox(bb1);
s2->SetBoundingBox(bb2);
PointVectorDouble pts;
pts.push_back(Vector2<double>(3.0, 5.0)); //inside
pts.push_back(Vector2<double>(3.0, 3.0)); //outside
s2->SetPointReadings(pts, true);
bool dirty = false;
s1->SetIsDirty(dirty);
s2->SetIsDirty(dirty);
double area = slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio(s1, s2);
EXPECT_EQ(area, 0.5);
delete s1;
delete s2;
}
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_

119
Localizations/Packages/slam_toolbox/slam_toolbox/test/process_constraints.py Normal file
View File

@@ -0,0 +1,119 @@
import math
import sys
import matplotlib.pyplot as plt
import random
'''
Purpose: This is a simple graphing tool to find the relationship between number of nodes
and number of constraints over time.
To use: add the following code to publishGraph and disable the optimizer printouts
std::map<karto::Name, std::vector<karto::Vertex<karto::LocalizedRangeScan>*> > vertices = mapper_->GetGraph()->GetVertices();
std::vector<karto::Vertex<karto::LocalizedRangeScan>*>::const_iterator it;
std::map<int, int> vertex_ctr;
for (it = vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].begin(); it != vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].end(); ++it)
{
int num = (*it)->GetEdges().size();
if (vertex_ctr.find(num) == vertex_ctr.end())
{
vertex_ctr[num] = 1;
}
vertex_ctr[num]++;
}
std::cout << "UpdateMap: Vertex count: " << std::endl;
std::map<int, int>::const_iterator it2;
for (it2 = vertex_ctr.begin(); it2 != vertex_ctr.end(); ++it2)
{
std::cout << it2->first << " constraints are in " << it2->second << " vertexes" << std::endl;
}
std::cout << std::endl;
'''
def readFileToList(filename):
with open(filename) as fp:
line = fp.readline()
lines = []
lines.append(line)
while line:
line = fp.readline()
lines.append(line)
return lines
def getSingleSets(lines):
measurements = []
measurement = []
for line in lines:
if line == "\n":
continue
if "UpdateMap: Vertex count:" in line:
measurements.append(measurement)
measurement = []
measurement.append(line)
return measurements[1:]
def processForData(measurements):
measurements_out = []
measurement_out = []
for measurement in measurements:
for m in measurement:
txt = m.split(" ")
nums = [r for r in txt if r.isdigit()]
if not nums:
continue
measurement_out.append(nums)
measurements_out.append(measurement_out)
measurement_out = []
return measurements_out
def plotData(data):
#plot lines
# give us the number of lines to create and unque items
max_size = 0
keys = []
for d in data:
for m in d:
if m[0] not in keys:
keys.append(m[0])
data_len = len(data)
dat = {}
for k in keys:
dat[k] = []
for d in data:
local_keys = []
for pt in d:
local_keys.append(pt[0])
dat[pt[0]].append(int(pt[1]))
for k in keys:
if k not in local_keys:
dat[k].append(0)
total_nodes = []
for d in data:
summ = 0
for pt in d:
summ = summ + int(pt[1])
total_nodes.append(summ)
for k in keys:
plt.plot( [10*i for i in range(0, len(dat[k]))], dat[k], marker='o', color=[random.random(),random.random(),random.random()], linewidth=2, label=k+' Constraints')
plt.plot( [10*i for i in range(0, len(dat[k]))], total_nodes, marker='o', color=[random.random(),random.random(),random.random()], linewidth=2, label='Total Num. Nodes')
plt.legend()
plt.xlabel("time (s)")
plt.ylabel("Node count")
#plt.yscale("log")
plt.show()
if __name__ == "__main__":
filename = sys.argv[1]
print ("reading file: " + filename)
listOfContents = readFileToList(filename)
listOfListOfMeasurements = getSingleSets(listOfContents)
data = processForData(listOfListOfMeasurements)
plotData(data)

25
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/CMakeLists.txt Normal file
View File

@@ -0,0 +1,25 @@
cmake_minimum_required(VERSION 3.5)
project(slam_toolbox_msgs)
set(MSG_DEPS std_srvs std_msgs geometry_msgs)
find_package(catkin REQUIRED COMPONENTS message_generation ${MSG_DEPS})
add_service_files(DIRECTORY srv
FILES
Pause.srv
ClearQueue.srv
ToggleInteractive.srv
Clear.srv
SaveMap.srv
LoopClosure.srv
MergeMaps.srv
AddSubmap.srv
DeserializePoseGraph.srv
SerializePoseGraph.srv
Reset.srv
)
generate_messages(DEPENDENCIES ${MSG_DEPS})
catkin_package(CATKIN_DEPENDS message_runtime ${MSG_DEPS})

24
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/package.xml Normal file
View File

@@ -0,0 +1,24 @@
<package format="2">
<name>slam_toolbox_msgs</name>
<description>
This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
</description>
<version>1.5.7</version>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<author>Steve Macenski</author>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>geometry_msgs</depend>
<exec_depend>message_runtime</exec_depend>
<export>
</export>
</package>

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/AddSubmap.srv Normal file
View File

@@ -0,0 +1,2 @@
string filename
---

3
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Clear.srv Normal file
View File

@@ -0,0 +1,3 @@
---

3
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ClearQueue.srv Normal file
View File

@@ -0,0 +1,3 @@
---
bool status

12
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/DeserializePoseGraph.srv Normal file
View File

@@ -0,0 +1,12 @@
int8 UNSET = 0
int8 START_AT_FIRST_NODE = 1
int8 START_AT_GIVEN_POSE = 2
int8 LOCALIZE_AT_POSE = 3
# inital_pose should be Map -> base_frame (parameter, generally base_link)
#
string filename
int8 match_type
geometry_msgs/Pose2D initial_pose
---

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/LoopClosure.srv Normal file
View File

@@ -0,0 +1,2 @@
---

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/MergeMaps.srv Normal file
View File

@@ -0,0 +1,2 @@
---

4
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Pause.srv Normal file
View File

@@ -0,0 +1,4 @@
# trigger pause toggle
---
bool status

8
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Reset.srv Normal file
View File

@@ -0,0 +1,8 @@
# Set this to 'true' to pause new measurements immediately after reset.
# Note: This is a set behaviour and not a toggle behaviour, contrary to Pause.srv service.
bool pause_new_measurements
---
# Result code defintions
uint8 RESULT_SUCCESS=0
uint8 result

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SaveMap.srv Normal file
View File

@@ -0,0 +1,2 @@
std_msgs/String name
---

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SerializePoseGraph.srv Normal file
View File

@@ -0,0 +1,2 @@
string filename
---

2
Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ToggleInteractive.srv Normal file
View File

@@ -0,0 +1,2 @@
---

58
Localizations/Packages/slam_toolbox/slam_toolbox_rviz/CMakeLists.txt Normal file
View File

@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.5)
project(slam_toolbox_rviz)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(catkin REQUIRED
COMPONENTS
rviz
pluginlib
slam_toolbox_msgs
)
include_directories(${catkin_INCLUDE_DIRS})
add_definitions(-DQT_NO_KEYWORDS)
#include_directories(${EIGEN3_INCLUDE_DIRS})
#add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS
${EIGEN3_INCLUDE_DIRS}
CATKIN_DEPENDS
rviz
slam_toolbox_msgs
)
find_package(Qt5 REQUIRED Core Widgets)
set(QT_LIBRARIES Qt5::Widgets)
add_definitions(-DQT_NO_KEYWORDS)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
set(SOURCE_FILES
src/slam_toolbox_rviz_plugin.cpp
)
qt_wrap_cpp(${PROJECT_NAME} MOC_FILES
src/slam_toolbox_rviz_plugin.h
)
add_library(${PROJECT_NAME} ${SOURCE_FILES} ${MOC_FILES})
target_link_libraries(${PROJECT_NAME} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${catkin_LIBRARIES})
#### Install
install(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(FILES rviz_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
add_dependencies(slam_toolbox_rviz slam_toolbox_msgs_generate_messages_cpp)

27
Localizations/Packages/slam_toolbox/slam_toolbox_rviz/package.xml Normal file
View File

@@ -0,0 +1,27 @@
<package format="2">
<name>slam_toolbox_rviz</name>
<description>
This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
</description>
<version>1.5.7</version>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<author>Steve Macenski</author>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>rviz</depend>
<depend>slam_toolbox_msgs</depend>
<depend>qtbase5-dev</depend>
<exec_depend>libqt5-core</exec_depend>
<exec_depend>libqt5-gui</exec_depend>
<exec_depend>libqt5-opengl</exec_depend>
<exec_depend>libqt5-widgets</exec_depend>
<export>
<rviz plugin="${prefix}/rviz_plugins.xml"/>
</export>
</package>

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