Initial import of Cartographer codebase.
Damon Kohler
commit
63c2321d26
|
|
@ -0,0 +1,7 @@
|
|||
# This is the list of Cartographer authors for copyright purposes.
|
||||
#
|
||||
# This does not necessarily list everyone who has contributed code, since in
|
||||
# some cases, their employer may be the copyright holder. To see the full list
|
||||
# of contributors, see the revision history in source control.
|
||||
Google Inc.
|
||||
and other contributors
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
Want to contribute? Great! First, read this page (including the small print at the end).
|
||||
|
||||
### Before you contribute
|
||||
Before we can use your code, you must sign the
|
||||
[Google Individual Contributor License Agreement]
|
||||
(https://cla.developers.google.com/about/google-individual)
|
||||
(CLA), which you can do online. The CLA is necessary mainly because you own the
|
||||
copyright to your changes, even after your contribution becomes part of our
|
||||
codebase, so we need your permission to use and distribute your code. We also
|
||||
need to be sure of various other things—for instance that you'll tell us if you
|
||||
know that your code infringes on other people's patents. You don't have to sign
|
||||
the CLA until after you've submitted your code for review and a member has
|
||||
approved it, but you must do it before we can put your code into our codebase.
|
||||
Before you start working on a larger contribution, you should get in touch with
|
||||
us first through the issue tracker with your idea so that we can help out and
|
||||
possibly guide you. Coordinating up front makes it much easier to avoid
|
||||
frustration later on.
|
||||
|
||||
### Code reviews
|
||||
All submissions, including submissions by project members, require review. We
|
||||
use Github pull requests for this purpose.
|
||||
|
||||
### The small print
|
||||
Contributions made by corporations are covered by a different agreement than
|
||||
the one above, the
|
||||
[Software Grant and Corporate Contributor License Agreement]
|
||||
(https://cla.developers.google.com/about/google-corporate).
|
||||
|
|
@ -0,0 +1,202 @@
|
|||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
|
|
@ -0,0 +1,153 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
project(google_cartographer)
|
||||
|
||||
set(PACKAGE_DEPENDENCIES
|
||||
eigen_conversions
|
||||
geometry_msgs
|
||||
google_cartographer_msgs
|
||||
roscpp
|
||||
rviz
|
||||
sensor_msgs
|
||||
tf2
|
||||
tf2_eigen
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-std=c++11")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
message_runtime
|
||||
${PACKAGE_DEPENDENCIES}
|
||||
)
|
||||
|
||||
find_package(PCL REQUIRED COMPONENTS common io)
|
||||
include_directories(${PCL_INCLUDE_DIRS})
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
add_definitions(${PCL_DEFINITIONS})
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
include_directories(${EIGEN_INCLUDE_DIRS})
|
||||
|
||||
find_package(cartographer REQUIRED)
|
||||
include_directories(${CARTOGRAPHER_INCLUDE_DIRS})
|
||||
link_directories(${CARTOGRAPHER_LIBRARY_DIRS})
|
||||
|
||||
find_package(ZLIB REQUIRED)
|
||||
include_directories(${ZLIB_INCLUDE_DIRS})
|
||||
|
||||
# TODO(pedrofernandez): CMake test rules are written differently in the core
|
||||
# library. Change the way it's done here and reenable testing.
|
||||
# enable_testing()
|
||||
# find_package(GTest REQUIRED)
|
||||
# include_directories(${GTEST_INCLUDE_DIRS})
|
||||
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
if(rviz_QT_VERSION VERSION_LESS "5")
|
||||
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
|
||||
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
|
||||
include(${QT_USE_FILE})
|
||||
else()
|
||||
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
|
||||
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
|
||||
set(QT_LIBRARIES Qt5::Widgets)
|
||||
endif()
|
||||
add_definitions(-DQT_NO_KEYWORDS)
|
||||
|
||||
find_package(Boost REQUIRED
|
||||
COMPONENTS
|
||||
system
|
||||
iostreams
|
||||
)
|
||||
add_definitions(${BOOST_DEFINITIONS})
|
||||
include_directories(${Boost_INCLUDE_DIRS})
|
||||
link_directories(${Boost_LIBRARY_DIRS})
|
||||
|
||||
add_executable(cartographer_node
|
||||
src/cartographer_node_main.cc
|
||||
src/node_constants.h
|
||||
src/msg_conversion.h
|
||||
src/msg_conversion.cc
|
||||
src/time_conversion.h
|
||||
src/time_conversion.cc
|
||||
)
|
||||
target_link_libraries(cartographer_node
|
||||
${CARTOGRAPHER_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(cartographer_rviz_submaps_visualization
|
||||
src/drawable_submap.cc
|
||||
src/drawable_submap.h
|
||||
src/submaps_display.cc
|
||||
src/submaps_display.h
|
||||
src/trajectory.h
|
||||
src/trajectory.cc
|
||||
)
|
||||
target_link_libraries(cartographer_rviz_submaps_visualization
|
||||
${Boost_LIBRARIES}
|
||||
${CARTOGRAPHER_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
${QT_LIBRARIES}
|
||||
${ZLIB_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(time_conversion_test
|
||||
src/time_conversion_test.cc
|
||||
src/time_conversion.h
|
||||
src/time_conversion.cc
|
||||
)
|
||||
|
||||
target_link_libraries(time_conversion_test
|
||||
${GTEST_BOTH_LIBRARIES}
|
||||
${CARTOGRAPHER_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_test(time_conversion_test time_conversion_test)
|
||||
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
|
||||
)
|
||||
|
||||
install(DIRECTORY urdf/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/
|
||||
)
|
||||
|
||||
install(DIRECTORY configuration_files/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/configuration_files/
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
cartographer_rviz_submaps_visualization
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES
|
||||
plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY ogre_media/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ogre_media}
|
||||
)
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
-- Copyright 2016 The Cartographer Authors
|
||||
--
|
||||
-- Licensed under the Apache License, Version 2.0 (the "License");
|
||||
-- you may not use this file except in compliance with the License.
|
||||
-- You may obtain a copy of the License at
|
||||
--
|
||||
-- http://www.apache.org/licenses/LICENSE-2.0
|
||||
--
|
||||
-- Unless required by applicable law or agreed to in writing, software
|
||||
-- distributed under the License is distributed on an "AS IS" BASIS,
|
||||
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
-- See the License for the specific language governing permissions and
|
||||
-- limitations under the License.
|
||||
|
||||
include "backpack_3d.lua"
|
||||
|
||||
options.trajectory_builder.scans_per_accumulation = 2
|
||||
|
||||
return options
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
-- Copyright 2016 The Cartographer Authors
|
||||
--
|
||||
-- Licensed under the Apache License, Version 2.0 (the "License");
|
||||
-- you may not use this file except in compliance with the License.
|
||||
-- You may obtain a copy of the License at
|
||||
--
|
||||
-- http://www.apache.org/licenses/LICENSE-2.0
|
||||
--
|
||||
-- Unless required by applicable law or agreed to in writing, software
|
||||
-- distributed under the License is distributed on an "AS IS" BASIS,
|
||||
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
-- See the License for the specific language governing permissions and
|
||||
-- limitations under the License.
|
||||
|
||||
include "trajectory_builder_3d.lua"
|
||||
include "sparse_pose_graph.lua"
|
||||
|
||||
options = {
|
||||
sparse_pose_graph = SPARSE_POSE_GRAPH,
|
||||
trajectory_builder = TRAJECTORY_BUILDER_3D,
|
||||
}
|
||||
|
||||
options.sparse_pose_graph.optimize_every_n_scans = 320
|
||||
options.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
|
||||
options.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
|
||||
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
|
||||
-- constraints.
|
||||
options.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
|
||||
options.sparse_pose_graph.constraint_builder.min_score = 0.62
|
||||
options.sparse_pose_graph.constraint_builder.log_matches = true
|
||||
|
||||
return options
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
-- Copyright 2016 The Cartographer Authors
|
||||
--
|
||||
-- Licensed under the Apache License, Version 2.0 (the "License");
|
||||
-- you may not use this file except in compliance with the License.
|
||||
-- You may obtain a copy of the License at
|
||||
--
|
||||
-- http://www.apache.org/licenses/LICENSE-2.0
|
||||
--
|
||||
-- Unless required by applicable law or agreed to in writing, software
|
||||
-- distributed under the License is distributed on an "AS IS" BASIS,
|
||||
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
-- See the License for the specific language governing permissions and
|
||||
-- limitations under the License.
|
||||
|
||||
include "trajectory_builder.lua"
|
||||
include "sparse_pose_graph.lua"
|
||||
|
||||
options = {
|
||||
sparse_pose_graph = SPARSE_POSE_GRAPH,
|
||||
trajectory_builder = TRAJECTORY_BUILDER,
|
||||
}
|
||||
|
||||
options.trajectory_builder.expect_imu_data = false
|
||||
options.trajectory_builder.use_online_correlative_scan_matching = true
|
||||
|
||||
return options
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<node pkg="nodelet" type="nodelet" name="velodyne_nodelet_manager"
|
||||
args="manager" />
|
||||
<node pkg="nodelet" type="nodelet" name="horizontal_driver_nodelet"
|
||||
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
|
||||
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
|
||||
<param name="model" value="VLP16"/>
|
||||
<param name="pcap" value=""/>
|
||||
<param name="read_once" value="false"/>
|
||||
<param name="read_fast" value="false"/>
|
||||
<param name="repeat_delay" value="0.0"/>
|
||||
<param name="rpm" value="600.0"/>
|
||||
<param name="port" value="2368" />
|
||||
<param name="frame_id" value="horizontal_vlp16_link"/>
|
||||
</node>
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="vertical_driver_nodelet"
|
||||
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
|
||||
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
|
||||
<param name="model" value="VLP16"/>
|
||||
<param name="pcap" value=""/>
|
||||
<param name="read_once" value="false"/>
|
||||
<param name="read_fast" value="false"/>
|
||||
<param name="repeat_delay" value="0.0"/>
|
||||
<param name="rpm" value="600.0"/>
|
||||
<param name="port" value="2369" />
|
||||
<param name="frame_id" value="vertical_vlp16_link"/>
|
||||
</node>
|
||||
|
||||
<remap from="/imu/imu" to="/imu" />
|
||||
<!-- TODO(hrapp): the IMU has a bug that makes it impossible to use arg for
|
||||
setting the parameter. so we work around this by setting the param
|
||||
directly -->
|
||||
<param name="/imu/frameId" value="imu_link"/>
|
||||
<include file="$(find imu_3dm_gx4)/launch/imu.launch">
|
||||
<arg name="frame_id" value="imu_link"/>
|
||||
</include>
|
||||
|
||||
<!-- VLP16 packets -> points -->
|
||||
<arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
|
||||
<arg name="min_range" default="0.4" />
|
||||
<arg name="max_range" default="130.0" />
|
||||
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_horizontal"
|
||||
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
|
||||
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
|
||||
<remap from="velodyne_points" to="horizontal_3d_laser" />
|
||||
<param name="calibration" value="$(arg calibration)"/>
|
||||
<param name="min_range" value="$(arg min_range)"/>
|
||||
<param name="max_range" value="$(arg max_range)"/>
|
||||
</node>
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_vertical"
|
||||
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
|
||||
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
|
||||
<remap from="velodyne_points" to="vertical_3d_laser" />
|
||||
<param name="calibration" value="$(arg calibration)"/>
|
||||
<param name="min_range" value="$(arg min_range)"/>
|
||||
<param name="max_range" value="$(arg max_range)"/>
|
||||
</node>
|
||||
|
||||
<param name="robot_description"
|
||||
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
|
||||
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||
type="robot_state_publisher" />
|
||||
|
||||
<node name="cartographer" pkg="google_cartographer"
|
||||
type="cartographer_node" output="screen" >
|
||||
<rosparam subst_value="true">
|
||||
map_frame: "map"
|
||||
tracking_frame: "base_link"
|
||||
configuration_files_directories: [
|
||||
"$(find google_cartographer)/configuration_files"
|
||||
]
|
||||
mapping_configuration_basename: "3d_mapping.lua"
|
||||
imu_topic: "/imu"
|
||||
laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"]
|
||||
laser_min_range_m: 0.
|
||||
laser_max_range_m: 30.
|
||||
laser_missing_echo_ray_length_m: 5.
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<param name="robot_description"
|
||||
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
|
||||
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||
type="robot_state_publisher" />
|
||||
|
||||
<node name="cartographer" pkg="google_cartographer"
|
||||
type="cartographer_node" output="screen" >
|
||||
<rosparam subst_value="true">
|
||||
# This node publishes the transformation between the tracking and map
|
||||
# frames.
|
||||
map_frame: "map"
|
||||
tracking_frame: "base_link"
|
||||
|
||||
# Lua configuration files are always searched in the cartographer
|
||||
# installation directory. We also ship configuration that work well for
|
||||
# the platforms that we used to collect our example data. You probably
|
||||
# want to add your own configuration overwrites in your own node
|
||||
# directories - you should add the path here as first entry then.
|
||||
configuration_files_directories: [
|
||||
"$(find google_cartographer)/configuration_files"
|
||||
]
|
||||
|
||||
# Configuration file for SLAM. The settings in here are tweaked to the
|
||||
# collection platform you are using.
|
||||
mapping_configuration_basename: "backpack_3d.lua"
|
||||
|
||||
imu_topic: "/imu"
|
||||
|
||||
# Laser min and max range. Everything not in this range will not be used for mapping.
|
||||
laser_min_range_m: 0.
|
||||
laser_max_range_m: 30.
|
||||
|
||||
# Missing laser echoes will be inserted as free space at this distance.
|
||||
laser_missing_echo_ray_length_m: 5.
|
||||
|
||||
# Only choose one in the next parameter block
|
||||
# laser_scan_2d_topic: "/horizontal_2d_laser"
|
||||
# multi_echo_laser_scan_2d_topic: "/horizontal_multiecho_2d_laser"
|
||||
laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- TODO(hrapp): still useful for occasional testing, but delete eventually.
|
||||
<node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping"
|
||||
output="screen">
|
||||
<param name="scan_topic" value="horizontal_2d_laser" />
|
||||
<param name="pub_map_odom_transform" value="true"/>
|
||||
<param name="map_frame" value="map" />
|
||||
<param name="base_frame" value="base_link" />
|
||||
<param name="odom_frame" value="base_link" />
|
||||
<param name="map_size" value="4096" />
|
||||
<param name="map_resolution" value="0.1" />
|
||||
</node>
|
||||
-->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<param name="robot_description"
|
||||
textfile="$(find google_cartographer)/urdf/turtlebot.urdf" />
|
||||
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||
type="robot_state_publisher" />
|
||||
|
||||
<node name="cartographer" pkg="google_cartographer"
|
||||
type="cartographer_node" output="screen" >
|
||||
<rosparam subst_value="true">
|
||||
map_frame: "map"
|
||||
tracking_frame: "base_link"
|
||||
configuration_files_directories: [
|
||||
"$(find google_cartographer)/configuration_files"
|
||||
]
|
||||
mapping_configuration_basename: "turtlebot.lua"
|
||||
imu_topic: "/imu"
|
||||
laser_scan_2d_topic: "/horizontal_2d_laser"
|
||||
laser_min_range_m: 0.
|
||||
laser_max_range_m: 30.
|
||||
laser_missing_echo_ray_length_m: 5.
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
fragment_program google_cartographer/glsl120/submap.frag glsl
|
||||
{
|
||||
source submap.frag
|
||||
}
|
||||
|
||||
vertex_program google_cartographer/glsl120/submap.vert glsl
|
||||
{
|
||||
source submap.vert
|
||||
}
|
||||
|
||||
fragment_program google_cartographer/glsl120/screen_blit.frag glsl
|
||||
{
|
||||
source screen_blit.frag
|
||||
}
|
||||
|
||||
vertex_program google_cartographer/glsl120/screen_blit.vert glsl
|
||||
{
|
||||
source screen_blit.vert
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#version 120
|
||||
|
||||
varying vec2 out_texture_coordinate;
|
||||
|
||||
uniform sampler2D u_texture;
|
||||
|
||||
void main()
|
||||
{
|
||||
vec2 texture_color = texture2D(u_texture, out_texture_coordinate).rg;
|
||||
float opacity = texture_color.g;
|
||||
float value = texture_color.r;
|
||||
gl_FragColor = vec4(value, value, value, opacity);
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#version 120
|
||||
|
||||
attribute vec4 vertex;
|
||||
attribute vec4 uv0;
|
||||
|
||||
varying vec2 out_texture_coordinate;
|
||||
|
||||
void main()
|
||||
{
|
||||
out_texture_coordinate = vec2(uv0);
|
||||
gl_Position = vertex;
|
||||
}
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#version 120
|
||||
|
||||
varying vec2 out_submap_texture_coordinate;
|
||||
|
||||
uniform sampler2D u_submap;
|
||||
uniform float u_alpha;
|
||||
|
||||
void main()
|
||||
{
|
||||
vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg;
|
||||
float value = texture_value.r;
|
||||
float alpha = texture_value.g;
|
||||
float is_known = step(1e-3, alpha + value);
|
||||
gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha);
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#version 120
|
||||
|
||||
attribute vec4 uv0;
|
||||
|
||||
varying vec2 out_submap_texture_coordinate;
|
||||
|
||||
void main()
|
||||
{
|
||||
out_submap_texture_coordinate = vec2(uv0);
|
||||
gl_Position = ftransform();
|
||||
}
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
material google_cartographer/ScreenBlit
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
vertex_program_ref google_cartographer/glsl120/screen_blit.vert {}
|
||||
|
||||
fragment_program_ref google_cartographer/glsl120/screen_blit.frag
|
||||
{
|
||||
param_named u_texture int 0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
material google_cartographer/Submap
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
vertex_program_ref google_cartographer/glsl120/submap.vert {}
|
||||
|
||||
fragment_program_ref google_cartographer/glsl120/submap.frag
|
||||
{
|
||||
param_named u_submap int 0
|
||||
param_named u_alpha float 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<package>
|
||||
<name>google_cartographer</name>
|
||||
<version>1.0.0</version>
|
||||
<description>The google_cartographer package.</description>
|
||||
<maintainer email="nobody@google.com">Google</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>eigen_conversions</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>google_cartographer_msgs</build_depend>
|
||||
<build_depend>libpcl-all-dev</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>pcl_conversions</build_depend>
|
||||
<build_depend>qtbase5-dev</build_depend>
|
||||
<build_depend>rosbag</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>roslib</build_depend>
|
||||
<build_depend>rviz</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>tf2</build_depend>
|
||||
<build_depend>tf2_eigen</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
|
||||
<run_depend>eigen_conversions</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>google_cartographer_msgs</run_depend>
|
||||
<run_depend>libpcl-all</run_depend>
|
||||
<run_depend>libqt5-core</run_depend>
|
||||
<run_depend>libqt5-gui</run_depend>
|
||||
<run_depend>libqt5-widgets</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>pcl_conversions</run_depend>
|
||||
<run_depend>rosbag</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>roslib</run_depend>
|
||||
<run_depend>rviz</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>tf2_eigen</run_depend>
|
||||
<run_depend>tf2</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
<rviz plugin="${prefix}/rviz_plugin_description.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<library path="lib/libcartographer_rviz_submaps_visualization">
|
||||
<class name="Cartographer Submap visualization"
|
||||
type="cartographer_ros::rviz::SubmapsDisplay"
|
||||
base_class_type="rviz::Display">
|
||||
<description>
|
||||
Displays Google Cartographer's submaps as a unified map in RViz.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
|
@ -0,0 +1,644 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <cstring>
|
||||
#include <queue>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/configuration_file_resolver.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/mutex.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/thread_pool.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping/proto/submaps.pb.h"
|
||||
#include "cartographer/mapping/sensor_collator.h"
|
||||
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/sparse_pose_graph.h"
|
||||
#include "cartographer/mapping_3d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "geometry_msgs/Transform.h"
|
||||
#include "geometry_msgs/TransformStamped.h"
|
||||
#include "glog/log_severity.h"
|
||||
#include "glog/logging.h"
|
||||
#include "google_cartographer_msgs/SubmapEntry.h"
|
||||
#include "google_cartographer_msgs/SubmapList.h"
|
||||
#include "google_cartographer_msgs/SubmapQuery.h"
|
||||
#include "google_cartographer_msgs/TrajectorySubmapList.h"
|
||||
#include "pcl/point_cloud.h"
|
||||
#include "pcl/point_types.h"
|
||||
#include "pcl_conversions/pcl_conversions.h"
|
||||
#include "ros/ros.h"
|
||||
#include "ros/serialization.h"
|
||||
#include "sensor_msgs/Imu.h"
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "sensor_msgs/MultiEchoLaserScan.h"
|
||||
#include "sensor_msgs/PointCloud2.h"
|
||||
#include "tf2_eigen/tf2_eigen.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "msg_conversion.h"
|
||||
#include "node_constants.h"
|
||||
#include "time_conversion.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace {
|
||||
|
||||
using ::cartographer::transform::Rigid3d;
|
||||
namespace proto = ::cartographer::sensor::proto;
|
||||
|
||||
// TODO(hrapp): Support multi trajectory mapping.
|
||||
constexpr int64 kTrajectoryId = 0;
|
||||
constexpr int kSubscriberQueueSize = 150;
|
||||
constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds
|
||||
constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds
|
||||
|
||||
Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) {
|
||||
return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
|
||||
transform.transform.translation.y,
|
||||
transform.transform.translation.z),
|
||||
Eigen::Quaterniond(transform.transform.rotation.w,
|
||||
transform.transform.rotation.x,
|
||||
transform.transform.rotation.y,
|
||||
transform.transform.rotation.z));
|
||||
}
|
||||
|
||||
// TODO(hrapp): move to msg_conversion.cc.
|
||||
geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) {
|
||||
geometry_msgs::Transform transform;
|
||||
transform.translation.x = rigid.translation().x();
|
||||
transform.translation.y = rigid.translation().y();
|
||||
transform.translation.z = rigid.translation().z();
|
||||
transform.rotation.w = rigid.rotation().w();
|
||||
transform.rotation.x = rigid.rotation().x();
|
||||
transform.rotation.y = rigid.rotation().y();
|
||||
transform.rotation.z = rigid.rotation().z();
|
||||
return transform;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) {
|
||||
geometry_msgs::Pose pose;
|
||||
pose.position.x = rigid.translation().x();
|
||||
pose.position.y = rigid.translation().y();
|
||||
pose.position.z = rigid.translation().z();
|
||||
pose.orientation.w = rigid.rotation().w();
|
||||
pose.orientation.x = rigid.rotation().x();
|
||||
pose.orientation.y = rigid.rotation().y();
|
||||
pose.orientation.z = rigid.rotation().z();
|
||||
return pose;
|
||||
}
|
||||
|
||||
// This type is a logical union, i.e. only one proto is actually filled in. It
|
||||
// is only used for time ordering sensor data before passing it on.
|
||||
enum class SensorType { kImu, kLaserScan, kLaserFan3D };
|
||||
struct SensorData {
|
||||
SensorData(const string& init_frame_id, proto::Imu init_imu)
|
||||
: type(SensorType::kImu), frame_id(init_frame_id), imu(init_imu) {}
|
||||
SensorData(const string& init_frame_id, proto::LaserScan init_laser_scan)
|
||||
: type(SensorType::kLaserScan),
|
||||
frame_id(init_frame_id),
|
||||
laser_scan(init_laser_scan) {}
|
||||
SensorData(const string& init_frame_id, proto::LaserFan3D init_laser_fan_3d)
|
||||
: type(SensorType::kLaserFan3D),
|
||||
frame_id(init_frame_id),
|
||||
laser_fan_3d(init_laser_fan_3d) {}
|
||||
|
||||
SensorType type;
|
||||
string frame_id;
|
||||
proto::Imu imu;
|
||||
proto::LaserScan laser_scan;
|
||||
proto::LaserFan3D laser_fan_3d;
|
||||
};
|
||||
|
||||
// Node that listens to all the sensor data that we are interested in and wires
|
||||
// it up to the SLAM.
|
||||
class Node {
|
||||
public:
|
||||
Node();
|
||||
Node(const Node&) = delete;
|
||||
Node& operator=(const Node&) = delete;
|
||||
|
||||
void SpinForever();
|
||||
void Initialize();
|
||||
|
||||
private:
|
||||
void HandleSensorData(int64 timestamp,
|
||||
std::unique_ptr<SensorData> sensor_data);
|
||||
void ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg);
|
||||
void LaserScanMessageCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
void MultiEchoLaserScanCallback(
|
||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
|
||||
void PointCloud2MessageCallback(
|
||||
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg);
|
||||
void AddImu(int64 timestamp, const string& frame_id, const proto::Imu& imu);
|
||||
void AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
|
||||
const proto::LaserScan& laser_scan);
|
||||
void AddLaserFan3D(int64 timestamp, const string& frame_id,
|
||||
const proto::LaserFan3D& laser_fan_3d);
|
||||
|
||||
template <typename T>
|
||||
const T GetParamOrDie(const string& name);
|
||||
|
||||
// Returns true if a transform for 'frame_id' to 'tracking_frame_' exists at
|
||||
// 'time'.
|
||||
bool CanTransform(ros::Time time, const string& frame_id);
|
||||
|
||||
Rigid3d LookupToTrackingTransformOrDie(ros::Time time,
|
||||
const string& frame_id);
|
||||
|
||||
bool HandleSubmapQuery(
|
||||
::google_cartographer_msgs::SubmapQuery::Request& request,
|
||||
::google_cartographer_msgs::SubmapQuery::Response& response);
|
||||
|
||||
void PublishSubmapList(int64 timestamp);
|
||||
void PublishPose(int64 timestamp);
|
||||
|
||||
// TODO(hrapp): Pull out the common functionality between this and MapWriter
|
||||
// into an open sourcable MapWriter.
|
||||
::cartographer::mapping::SensorCollator<SensorData> sensor_collator_;
|
||||
ros::NodeHandle node_handle_;
|
||||
ros::Subscriber imu_subscriber_;
|
||||
ros::Subscriber laser_2d_subscriber_;
|
||||
std::vector<ros::Subscriber> laser_3d_subscribers_;
|
||||
string tracking_frame_;
|
||||
string map_frame_;
|
||||
double laser_min_range_m_;
|
||||
double laser_max_range_m_;
|
||||
double laser_missing_echo_ray_length_m_;
|
||||
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_;
|
||||
tf2_ros::TransformBroadcaster tf_broadcaster_;
|
||||
::cartographer::common::ThreadPool thread_pool_;
|
||||
int64 last_pose_publish_timestamp_;
|
||||
::cartographer::common::Mutex mutex_;
|
||||
std::unique_ptr<::cartographer::mapping::GlobalTrajectoryBuilderInterface>
|
||||
trajectory_builder_ GUARDED_BY(mutex_);
|
||||
std::deque<::cartographer::mapping::TrajectoryNode::ConstantData>
|
||||
constant_node_data_ GUARDED_BY(mutex_);
|
||||
std::unique_ptr<::cartographer::mapping::SparsePoseGraph> sparse_pose_graph_;
|
||||
|
||||
::ros::Publisher submap_list_publisher_;
|
||||
int64 last_submap_list_publish_timestamp_;
|
||||
::ros::ServiceServer submap_query_server_;
|
||||
};
|
||||
|
||||
Node::Node()
|
||||
: node_handle_("~"),
|
||||
tf_buffer_(ros::Duration(1000)),
|
||||
tf_(tf_buffer_),
|
||||
thread_pool_(10),
|
||||
last_submap_list_publish_timestamp_(0),
|
||||
last_pose_publish_timestamp_(0) {}
|
||||
|
||||
bool Node::CanTransform(ros::Time time, const string& frame_id) {
|
||||
return tf_buffer_.canTransform(tracking_frame_, frame_id, time);
|
||||
}
|
||||
|
||||
Rigid3d Node::LookupToTrackingTransformOrDie(ros::Time time,
|
||||
const string& frame_id) {
|
||||
geometry_msgs::TransformStamped stamped_transform;
|
||||
try {
|
||||
stamped_transform = tf_buffer_.lookupTransform(tracking_frame_, frame_id,
|
||||
time, ros::Duration(1.));
|
||||
} catch (tf2::TransformException& ex) {
|
||||
LOG(FATAL) << "Timed out while waiting for transform: " << frame_id
|
||||
<< " -> " << tracking_frame_ << ": " << ex.what();
|
||||
}
|
||||
return ToRidig3d(stamped_transform);
|
||||
}
|
||||
|
||||
void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(*msg));
|
||||
sensor_collator_.AddSensorData(
|
||||
kTrajectoryId,
|
||||
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
|
||||
imu_subscriber_.getTopic(), std::move(sensor_data));
|
||||
}
|
||||
|
||||
void Node::AddImu(const int64 timestamp, const string& frame_id,
|
||||
const proto::Imu& imu) {
|
||||
const ::cartographer::common::Time time =
|
||||
::cartographer::common::FromUniversal(timestamp);
|
||||
|
||||
if (!CanTransform(ToRos(time), frame_id)) {
|
||||
LOG(WARNING) << "Cannot transform to " << frame_id;
|
||||
return;
|
||||
}
|
||||
const Rigid3d sensor_to_tracking =
|
||||
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
|
||||
CHECK(sensor_to_tracking.translation().norm() < 1e-5)
|
||||
<< "The IMU is not colocated with the tracking frame. This makes it hard "
|
||||
"and inprecise to transform its linear accelaration into the "
|
||||
"tracking_frame and will decrease the quality of the SLAM.";
|
||||
trajectory_builder_->AddImuData(
|
||||
time, sensor_to_tracking.rotation() *
|
||||
::cartographer::transform::ToEigen(imu.linear_acceleration()),
|
||||
sensor_to_tracking.rotation() *
|
||||
::cartographer::transform::ToEigen(imu.angular_velocity()));
|
||||
}
|
||||
|
||||
void Node::LaserScanMessageCallback(
|
||||
const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(*msg));
|
||||
sensor_collator_.AddSensorData(
|
||||
kTrajectoryId,
|
||||
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
|
||||
laser_2d_subscriber_.getTopic(), std::move(sensor_data));
|
||||
}
|
||||
|
||||
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
|
||||
const proto::LaserScan& laser_scan) {
|
||||
const ::cartographer::common::Time time =
|
||||
::cartographer::common::FromUniversal(timestamp);
|
||||
if (!CanTransform(ToRos(time), frame_id)) {
|
||||
LOG(WARNING) << "Cannot transform to " << frame_id;
|
||||
return;
|
||||
}
|
||||
const Rigid3d sensor_to_tracking =
|
||||
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
|
||||
|
||||
// TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
|
||||
const ::cartographer::sensor::LaserFan laser_fan =
|
||||
::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
|
||||
laser_max_range_m_,
|
||||
laser_missing_echo_ray_length_m_);
|
||||
|
||||
const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
|
||||
::cartographer::sensor::ToLaserFan3D(laser_fan),
|
||||
sensor_to_tracking.cast<float>());
|
||||
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
|
||||
}
|
||||
|
||||
void Node::MultiEchoLaserScanCallback(
|
||||
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||
// TODO(hrapp): Do something useful.
|
||||
LOG(INFO) << "LaserScan message: " << msg->header.stamp;
|
||||
}
|
||||
|
||||
void Node::PointCloud2MessageCallback(
|
||||
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
pcl::PointCloud<pcl::PointXYZ> pcl_points;
|
||||
pcl::fromROSMsg(*msg, pcl_points);
|
||||
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(pcl_points));
|
||||
sensor_collator_.AddSensorData(
|
||||
kTrajectoryId,
|
||||
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
|
||||
std::move(sensor_data));
|
||||
}
|
||||
|
||||
void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
|
||||
const proto::LaserFan3D& laser_fan_3d) {
|
||||
const ::cartographer::common::Time time =
|
||||
::cartographer::common::FromUniversal(timestamp);
|
||||
if (!CanTransform(ToRos(time), frame_id)) {
|
||||
LOG(WARNING) << "Cannot transform to " << frame_id;
|
||||
return;
|
||||
}
|
||||
const Rigid3d sensor_to_tracking =
|
||||
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
|
||||
|
||||
trajectory_builder_->AddLaserFan3D(
|
||||
time, ::cartographer::sensor::TransformLaserFan3D(
|
||||
::cartographer::sensor::FromProto(laser_fan_3d),
|
||||
sensor_to_tracking.cast<float>()));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
const T Node::GetParamOrDie(const string& name) {
|
||||
CHECK(node_handle_.hasParam(name)) << "Required parameter '" << name
|
||||
<< "' is unset.";
|
||||
T value;
|
||||
node_handle_.getParam(name, value);
|
||||
return value;
|
||||
}
|
||||
|
||||
void Node::Initialize() {
|
||||
tracking_frame_ = GetParamOrDie<string>("tracking_frame");
|
||||
map_frame_ = GetParamOrDie<string>("map_frame");
|
||||
laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m");
|
||||
laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m");
|
||||
laser_missing_echo_ray_length_m_ =
|
||||
GetParamOrDie<double>("laser_missing_echo_ray_length_m");
|
||||
|
||||
// Subscribe to the IMU.
|
||||
const string imu_topic = GetParamOrDie<string>("imu_topic");
|
||||
imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize,
|
||||
&Node::ImuMessageCallback, this);
|
||||
std::unordered_set<string> expected_sensor_identifiers;
|
||||
expected_sensor_identifiers.insert(imu_topic);
|
||||
|
||||
// Subscribe to exactly one laser.
|
||||
const bool has_laser_scan_2d = node_handle_.hasParam("laser_scan_2d_topic");
|
||||
const bool has_multi_echo_laser_scan_2d =
|
||||
node_handle_.hasParam("multi_echo_laser_scan_2d_topic");
|
||||
const bool has_laser_scan_3d = node_handle_.hasParam("laser_scan_3d_topics");
|
||||
|
||||
CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + has_laser_scan_3d ==
|
||||
1)
|
||||
<< "Parameters 'laser_scan_2d_topic', 'multi_echo_laser_scan_2d_topic' "
|
||||
"and 'laser_scan_3d_topics' are mutually exclusive, but one is "
|
||||
"required.";
|
||||
|
||||
if (has_laser_scan_2d) {
|
||||
const string topic = GetParamOrDie<string>("laser_scan_2d_topic");
|
||||
laser_2d_subscriber_ = node_handle_.subscribe(
|
||||
topic, kSubscriberQueueSize, &Node::LaserScanMessageCallback, this);
|
||||
expected_sensor_identifiers.insert(topic);
|
||||
}
|
||||
if (has_multi_echo_laser_scan_2d) {
|
||||
const string topic =
|
||||
GetParamOrDie<string>("multi_echo_laser_scan_2d_topic");
|
||||
laser_2d_subscriber_ = node_handle_.subscribe(
|
||||
topic, kSubscriberQueueSize, &Node::MultiEchoLaserScanCallback, this);
|
||||
expected_sensor_identifiers.insert(topic);
|
||||
}
|
||||
|
||||
auto file_resolver = ::cartographer::common::make_unique<
|
||||
::cartographer::common::ConfigurationFileResolver>(
|
||||
GetParamOrDie<std::vector<string>>("configuration_files_directories"));
|
||||
const string code = file_resolver->GetFileContentOrDie(
|
||||
GetParamOrDie<string>("mapping_configuration_basename"));
|
||||
|
||||
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||
code, std::move(file_resolver), nullptr);
|
||||
if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) {
|
||||
auto sparse_pose_graph_2d = ::cartographer::common::make_unique<
|
||||
::cartographer::mapping_2d::SparsePoseGraph>(
|
||||
::cartographer::mapping::CreateSparsePoseGraphOptions(
|
||||
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()),
|
||||
&thread_pool_, &constant_node_data_);
|
||||
trajectory_builder_ = ::cartographer::common::make_unique<
|
||||
::cartographer::mapping_2d::GlobalTrajectoryBuilder>(
|
||||
::cartographer::mapping_2d::CreateLocalTrajectoryBuilderOptions(
|
||||
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()),
|
||||
sparse_pose_graph_2d.get());
|
||||
sparse_pose_graph_ = std::move(sparse_pose_graph_2d);
|
||||
}
|
||||
|
||||
if (has_laser_scan_3d) {
|
||||
const auto topics =
|
||||
GetParamOrDie<std::vector<string>>("laser_scan_3d_topics");
|
||||
for (const auto& topic : topics) {
|
||||
laser_3d_subscribers_.push_back(node_handle_.subscribe(
|
||||
topic, kSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
PointCloud2MessageCallback(topic, msg);
|
||||
})));
|
||||
expected_sensor_identifiers.insert(topic);
|
||||
}
|
||||
auto sparse_pose_graph_3d = ::cartographer::common::make_unique<
|
||||
::cartographer::mapping_3d::SparsePoseGraph>(
|
||||
::cartographer::mapping::CreateSparsePoseGraphOptions(
|
||||
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()),
|
||||
&thread_pool_, &constant_node_data_);
|
||||
trajectory_builder_ = ::cartographer::common::make_unique<
|
||||
::cartographer::mapping_3d::GlobalTrajectoryBuilder>(
|
||||
::cartographer::mapping_3d::CreateLocalTrajectoryBuilderOptions(
|
||||
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()),
|
||||
sparse_pose_graph_3d.get());
|
||||
sparse_pose_graph_ = std::move(sparse_pose_graph_3d);
|
||||
}
|
||||
CHECK(sparse_pose_graph_ != nullptr);
|
||||
|
||||
// TODO(hrapp): Add odometry subscribers here.
|
||||
|
||||
sensor_collator_.AddTrajectory(
|
||||
kTrajectoryId, expected_sensor_identifiers,
|
||||
[this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
|
||||
HandleSensorData(timestamp, std::move(sensor_data));
|
||||
});
|
||||
|
||||
submap_list_publisher_ =
|
||||
node_handle_.advertise<::google_cartographer_msgs::SubmapList>(
|
||||
kSubmapListTopic, 10);
|
||||
submap_query_server_ = node_handle_.advertiseService(
|
||||
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
|
||||
}
|
||||
|
||||
bool Node::HandleSubmapQuery(
|
||||
::google_cartographer_msgs::SubmapQuery::Request& request,
|
||||
::google_cartographer_msgs::SubmapQuery::Response& response) {
|
||||
if (request.trajectory_id != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
::cartographer::common::MutexLocker lock(&mutex_);
|
||||
// TODO(hrapp): return error messages and extract common code from MapBuilder.
|
||||
::cartographer::mapping::Submaps* submaps = trajectory_builder_->submaps();
|
||||
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
::cartographer::mapping::proto::SubmapQuery::Response response_proto;
|
||||
response_proto.set_submap_id(request.submap_id);
|
||||
response_proto.set_submap_version(
|
||||
submaps->Get(request.submap_id)->end_laser_fan_index);
|
||||
const std::vector<::cartographer::transform::Rigid3d> submap_transforms =
|
||||
sparse_pose_graph_->GetSubmapTransforms(*submaps);
|
||||
|
||||
submaps->SubmapToProto(request.submap_id,
|
||||
sparse_pose_graph_->GetTrajectoryNodes(),
|
||||
submap_transforms[request.submap_id], &response_proto);
|
||||
|
||||
response.submap_id = response_proto.submap_id();
|
||||
response.submap_version = response_proto.submap_version();
|
||||
response.cells.insert(response.cells.begin(), response_proto.cells().begin(),
|
||||
response_proto.cells().end());
|
||||
response.width = response_proto.width();
|
||||
response.height = response_proto.height();
|
||||
response.resolution = response_proto.resolution();
|
||||
|
||||
auto pose = ::cartographer::transform::ToRigid3(response_proto.slice_pose());
|
||||
response.slice_pose.position.x =
|
||||
response_proto.slice_pose().translation().x();
|
||||
response.slice_pose.position.y =
|
||||
response_proto.slice_pose().translation().y();
|
||||
response.slice_pose.position.z =
|
||||
response_proto.slice_pose().translation().z();
|
||||
response.slice_pose.orientation.x =
|
||||
response_proto.slice_pose().rotation().x();
|
||||
response.slice_pose.orientation.y =
|
||||
response_proto.slice_pose().rotation().y();
|
||||
response.slice_pose.orientation.z =
|
||||
response_proto.slice_pose().rotation().z();
|
||||
response.slice_pose.orientation.w =
|
||||
response_proto.slice_pose().rotation().w();
|
||||
return true;
|
||||
}
|
||||
|
||||
void Node::PublishSubmapList(int64 timestamp) {
|
||||
::cartographer::common::MutexLocker lock(&mutex_);
|
||||
const ::cartographer::mapping::Submaps* submaps =
|
||||
trajectory_builder_->submaps();
|
||||
const std::vector<::cartographer::transform::Rigid3d> submap_transforms =
|
||||
sparse_pose_graph_->GetSubmapTransforms(*submaps);
|
||||
CHECK_EQ(submap_transforms.size(), submaps->size());
|
||||
|
||||
::google_cartographer_msgs::TrajectorySubmapList ros_trajectory;
|
||||
for (int i = 0; i != submaps->size(); ++i) {
|
||||
::google_cartographer_msgs::SubmapEntry ros_submap;
|
||||
ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index;
|
||||
ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]);
|
||||
ros_trajectory.submap.push_back(ros_submap);
|
||||
}
|
||||
|
||||
::google_cartographer_msgs::SubmapList ros_submap_list;
|
||||
ros_submap_list.trajectory.push_back(ros_trajectory);
|
||||
submap_list_publisher_.publish(ros_submap_list);
|
||||
last_submap_list_publish_timestamp_ = timestamp;
|
||||
}
|
||||
|
||||
void Node::PublishPose(int64 timestamp) {
|
||||
::cartographer::common::MutexLocker lock(&mutex_);
|
||||
const ::cartographer::mapping::Submaps* submaps =
|
||||
trajectory_builder_->submaps();
|
||||
const ::cartographer::transform::Rigid3d odometry_to_map =
|
||||
sparse_pose_graph_->GetOdometryToMapTransform(*submaps);
|
||||
const auto& pose_estimate = trajectory_builder_->pose_estimate();
|
||||
|
||||
const ::cartographer::transform::Rigid3d pose =
|
||||
odometry_to_map * pose_estimate.pose;
|
||||
const ::cartographer::common::Time time =
|
||||
::cartographer::common::FromUniversal(timestamp);
|
||||
|
||||
geometry_msgs::TransformStamped stamped_transform;
|
||||
stamped_transform.header.stamp = ToRos(time);
|
||||
stamped_transform.header.frame_id = map_frame_;
|
||||
stamped_transform.child_frame_id = tracking_frame_;
|
||||
stamped_transform.transform = ToGeometryMsgTransform(pose);
|
||||
tf_broadcaster_.sendTransform(stamped_transform);
|
||||
last_pose_publish_timestamp_ = timestamp;
|
||||
}
|
||||
|
||||
void Node::HandleSensorData(const int64 timestamp,
|
||||
std::unique_ptr<SensorData> sensor_data) {
|
||||
if (last_submap_list_publish_timestamp_ + kSubmapPublishPeriodInUts <
|
||||
timestamp) {
|
||||
PublishSubmapList(timestamp);
|
||||
}
|
||||
|
||||
if (last_pose_publish_timestamp_ + kPosePublishPeriodInUts < timestamp) {
|
||||
PublishPose(timestamp);
|
||||
}
|
||||
|
||||
switch (sensor_data->type) {
|
||||
case SensorType::kImu:
|
||||
AddImu(timestamp, sensor_data->frame_id, sensor_data->imu);
|
||||
return;
|
||||
|
||||
case SensorType::kLaserScan:
|
||||
AddHorizontalLaserFan(timestamp, sensor_data->frame_id,
|
||||
sensor_data->laser_scan);
|
||||
return;
|
||||
|
||||
case SensorType::kLaserFan3D:
|
||||
AddLaserFan3D(timestamp, sensor_data->frame_id,
|
||||
sensor_data->laser_fan_3d);
|
||||
return;
|
||||
}
|
||||
LOG(FATAL);
|
||||
}
|
||||
|
||||
void Node::SpinForever() { ros::spin(); }
|
||||
|
||||
void Run() {
|
||||
Node node;
|
||||
node.Initialize();
|
||||
node.SpinForever();
|
||||
}
|
||||
|
||||
const char* GetBasename(const char* filepath) {
|
||||
const char* base = strrchr(filepath, '/');
|
||||
return base ? (base + 1) : filepath;
|
||||
}
|
||||
|
||||
// Makes Google logging use ROS logging for output while an instance of this
|
||||
// class exists.
|
||||
class ScopedRosLogSink : public google::LogSink {
|
||||
public:
|
||||
ScopedRosLogSink() : will_die_(false) { AddLogSink(this); }
|
||||
~ScopedRosLogSink() override { RemoveLogSink(this); }
|
||||
|
||||
void send(google::LogSeverity severity, const char* filename,
|
||||
const char* base_filename, int line, const struct ::tm* tm_time,
|
||||
const char* message, size_t message_len) override {
|
||||
const std::string message_string = google::LogSink::ToString(
|
||||
severity, GetBasename(filename), line, tm_time, message, message_len);
|
||||
switch (severity) {
|
||||
case google::GLOG_INFO:
|
||||
ROS_INFO_STREAM(message_string);
|
||||
break;
|
||||
|
||||
case google::GLOG_WARNING:
|
||||
ROS_WARN_STREAM(message_string);
|
||||
break;
|
||||
|
||||
case google::GLOG_ERROR:
|
||||
ROS_ERROR_STREAM(message_string);
|
||||
break;
|
||||
|
||||
case google::GLOG_FATAL:
|
||||
ROS_FATAL_STREAM(message_string);
|
||||
will_die_ = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void WaitTillSent() override {
|
||||
if (will_die_) {
|
||||
// Arbirarily give ROS some time to actually publish our message.
|
||||
std::this_thread::sleep_for(
|
||||
::cartographer::common::FromMilliseconds(1000));
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
bool will_die_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
} // namespace cartographer_ros
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
|
||||
ros::init(argc, argv, "cartographer_node");
|
||||
ros::start();
|
||||
|
||||
::cartographer_ros::ScopedRosLogSink ros_log_sink;
|
||||
::cartographer_ros::Run();
|
||||
ros::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,269 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "drawable_submap.h"
|
||||
|
||||
#include <OgreGpuProgramParams.h>
|
||||
#include <OgreImage.h>
|
||||
#include <cartographer/common/port.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rviz/display_context.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <chrono>
|
||||
#include <future>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
|
||||
constexpr char kMapFrame[] = "/map";
|
||||
constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
|
||||
constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
|
||||
constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
|
||||
constexpr char kSubmapSourceMaterialName[] = "google_cartographer/Submap";
|
||||
|
||||
// Distance before which the submap will be shown at full opacity, and distance
|
||||
// over which the submap will then fade out.
|
||||
constexpr double kFadeOutStartDistanceInMeters = 1.;
|
||||
constexpr double kFadeOutDistanceInMeters = 2.;
|
||||
constexpr float kAlphaUpdateThreshold = 0.2f;
|
||||
|
||||
std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) {
|
||||
return std::to_string(trajectory_id) + "-" + std::to_string(submap_id);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
|
||||
::rviz::FrameManager* const frame_manager,
|
||||
Ogre::SceneManager* const scene_manager)
|
||||
: frame_manager_(frame_manager),
|
||||
scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
|
||||
manual_object_(scene_manager->createManualObject(
|
||||
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
|
||||
submap_id_(submap_id),
|
||||
trajectory_id_(trajectory_id),
|
||||
last_query_timestamp_(0),
|
||||
query_in_progress_(false),
|
||||
texture_count_(0),
|
||||
current_alpha_(0) {
|
||||
material_ = Ogre::MaterialManager::getSingleton().getByName(
|
||||
kSubmapSourceMaterialName);
|
||||
material_ = material_->clone(kSubmapMaterialPrefix +
|
||||
GetSubmapIdentifier(trajectory_id_, submap_id));
|
||||
material_->setReceiveShadows(false);
|
||||
material_->getTechnique(0)->setLightingEnabled(false);
|
||||
material_->setDepthWriteEnabled(false);
|
||||
scene_node_->attachObject(manual_object_);
|
||||
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(OnRequestSuccess()));
|
||||
}
|
||||
|
||||
DrawableSubmap::~DrawableSubmap() {
|
||||
Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
|
||||
if (!texture_.isNull()) {
|
||||
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
|
||||
texture_.setNull();
|
||||
}
|
||||
}
|
||||
|
||||
bool DrawableSubmap::Update(
|
||||
const ::google_cartographer_msgs::SubmapEntry& metadata,
|
||||
ros::ServiceClient* const client) {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
tf::poseMsgToEigen(metadata.pose, submap_pose_);
|
||||
const bool newer_version_available = version_ < metadata.submap_version;
|
||||
const std::chrono::milliseconds now =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch());
|
||||
const bool recently_queried =
|
||||
last_query_timestamp_ + kMinQueryDelayInMs > now;
|
||||
if (!newer_version_available || recently_queried || query_in_progress_) {
|
||||
return false;
|
||||
}
|
||||
query_in_progress_ = true;
|
||||
last_query_timestamp_ = now;
|
||||
last_query_slice_height_ = metadata.pose.position.z;
|
||||
QuerySubmap(submap_id_, trajectory_id_, client);
|
||||
return true;
|
||||
}
|
||||
|
||||
void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
|
||||
ros::ServiceClient* const client) {
|
||||
rpc_request_future_ = std::async(
|
||||
std::launch::async, [this, submap_id, trajectory_id, client]() {
|
||||
::google_cartographer_msgs::SubmapQuery srv;
|
||||
srv.request.submap_id = submap_id;
|
||||
srv.request.trajectory_id = trajectory_id;
|
||||
if (client->call(srv)) {
|
||||
response_.reset(new ::google_cartographer_msgs::SubmapQuery::Response(
|
||||
srv.response));
|
||||
Q_EMIT RequestSucceeded();
|
||||
} else {
|
||||
OnRequestFailure();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
void DrawableSubmap::OnRequestSuccess() {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
version_ = response_->submap_version;
|
||||
resolution_ = response_->resolution;
|
||||
width_ = response_->width;
|
||||
height_ = response_->height;
|
||||
slice_height_ = last_query_slice_height_;
|
||||
std::string compressed_cells(response_->cells.begin(),
|
||||
response_->cells.end());
|
||||
cells_.clear();
|
||||
::cartographer::common::FastGunzipString(compressed_cells, &cells_);
|
||||
Eigen::Affine3d slice_pose;
|
||||
tf::poseMsgToEigen(response_->slice_pose, slice_pose);
|
||||
tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
|
||||
response_.reset();
|
||||
query_in_progress_ = false;
|
||||
UpdateSceneNode();
|
||||
}
|
||||
|
||||
void DrawableSubmap::OnRequestFailure() {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
query_in_progress_ = false;
|
||||
}
|
||||
|
||||
bool DrawableSubmap::QueryInProgress() {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
return query_in_progress_;
|
||||
}
|
||||
|
||||
void DrawableSubmap::UpdateSceneNode() {
|
||||
// The call to Ogre's loadRawData below does not work with an RG texture,
|
||||
// therefore we create an RGB one whose blue channel is always 0.
|
||||
std::vector<char> rgb;
|
||||
for (int i = 0; i < height_; ++i) {
|
||||
for (int j = 0; j < width_; ++j) {
|
||||
auto r = cells_[(i * width_ + j) * 2];
|
||||
auto g = cells_[(i * width_ + j) * 2 + 1];
|
||||
rgb.push_back(r);
|
||||
rgb.push_back(g);
|
||||
rgb.push_back(0.);
|
||||
}
|
||||
}
|
||||
|
||||
manual_object_->clear();
|
||||
const float metric_width = resolution_ * width_;
|
||||
const float metric_height = resolution_ * height_;
|
||||
|
||||
manual_object_->begin(material_->getName(),
|
||||
Ogre::RenderOperation::OT_TRIANGLE_LIST);
|
||||
{
|
||||
{
|
||||
// Bottom left
|
||||
manual_object_->position(-metric_height, 0.0f, 0.0f);
|
||||
manual_object_->textureCoord(0.0f, 1.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// Bottom right
|
||||
manual_object_->position(-metric_height, -metric_width, 0.0f);
|
||||
manual_object_->textureCoord(1.0f, 1.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// Top left
|
||||
manual_object_->position(0.0f, 0.0f, 0.0f);
|
||||
manual_object_->textureCoord(0.0f, 0.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// Top left
|
||||
manual_object_->position(0.0f, 0.0f, 0.0f);
|
||||
manual_object_->textureCoord(0.0f, 0.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// Bottom right
|
||||
manual_object_->position(-metric_height, -metric_width, 0.0f);
|
||||
manual_object_->textureCoord(1.0f, 1.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// Top right
|
||||
manual_object_->position(0.0f, -metric_width, 0.0f);
|
||||
manual_object_->textureCoord(1.0f, 0.0f);
|
||||
manual_object_->normal(0.0f, 0.0f, 1.0f);
|
||||
}
|
||||
}
|
||||
|
||||
manual_object_->end();
|
||||
|
||||
Ogre::DataStreamPtr pixel_stream;
|
||||
pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
|
||||
|
||||
if (!texture_.isNull()) {
|
||||
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
|
||||
texture_.setNull();
|
||||
}
|
||||
const std::string texture_name =
|
||||
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_) +
|
||||
std::to_string(texture_count_);
|
||||
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
|
||||
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
|
||||
pixel_stream, width_, height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
|
||||
++texture_count_;
|
||||
|
||||
Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
|
||||
pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
|
||||
Ogre::TextureUnitState* const texture_unit =
|
||||
pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0)
|
||||
: pass->createTextureUnitState();
|
||||
|
||||
texture_unit->setTextureName(texture_->getName());
|
||||
texture_unit->setTextureFiltering(Ogre::TFO_NONE);
|
||||
}
|
||||
|
||||
void DrawableSubmap::Transform(const ros::Time& ros_time) {
|
||||
Ogre::Vector3 position;
|
||||
Ogre::Quaternion orientation;
|
||||
frame_manager_->transform(kMapFrame, ros_time, transformed_pose_, position,
|
||||
orientation);
|
||||
scene_node_->setPosition(position);
|
||||
scene_node_->setOrientation(orientation);
|
||||
}
|
||||
|
||||
void DrawableSubmap::SetAlpha(const double current_tracking_z) {
|
||||
const double distance_z = std::abs(slice_height_ - current_tracking_z);
|
||||
const double fade_distance =
|
||||
std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
|
||||
const float alpha =
|
||||
(float)std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters);
|
||||
|
||||
const Ogre::GpuProgramParametersSharedPtr parameters =
|
||||
material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
|
||||
parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha));
|
||||
}
|
||||
|
||||
float DrawableSubmap::UpdateAlpha(const float target_alpha) {
|
||||
if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
|
||||
target_alpha == 0.f || target_alpha == 1.f) {
|
||||
current_alpha_ = target_alpha;
|
||||
}
|
||||
return current_alpha_;
|
||||
}
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_
|
||||
|
||||
#include <OgreManualObject.h>
|
||||
#include <OgreMaterial.h>
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreSceneNode.h>
|
||||
#include <OgreTexture.h>
|
||||
#include <cartographer/common/mutex.h>
|
||||
#include <google_cartographer_msgs/SubmapEntry.h>
|
||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rviz/display_context.h>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <future>
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
// Contains all the information needed to render a submap onto the final
|
||||
// texture representing the whole map.
|
||||
class DrawableSubmap : public QObject {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
// Each submap is identified by a 'trajectory_id' plus a 'submap_id'. The
|
||||
// 'frame_manager' is needed to transform the scene node before rendering
|
||||
// onto the offscreen texture. 'scene_manager' is the Ogre scene manager which
|
||||
// contains all submaps.
|
||||
DrawableSubmap(int submap_id, int trajectory_id,
|
||||
::rviz::FrameManager* frame_manager,
|
||||
Ogre::SceneManager* scene_manager);
|
||||
~DrawableSubmap() override;
|
||||
DrawableSubmap(const DrawableSubmap&) = delete;
|
||||
DrawableSubmap& operator=(const DrawableSubmap&) = delete;
|
||||
|
||||
// 'submap_entry' contains metadata which is used to find out whether this
|
||||
// 'DrawableSubmap' should update itself. If an update is needed, it will send
|
||||
// an RPC using 'client' to request the new data for the submap.
|
||||
bool Update(const ::google_cartographer_msgs::SubmapEntry& submap_entry,
|
||||
ros::ServiceClient* client);
|
||||
|
||||
// Returns whether an RPC is in progress.
|
||||
bool QueryInProgress();
|
||||
|
||||
// Transforms the scene node for this submap before being rendered onto a
|
||||
// texture.
|
||||
void Transform(const ros::Time& ros_time);
|
||||
|
||||
// Sets the alpha of the submap taking into account its slice height and the
|
||||
// 'current_tracking_z'.
|
||||
void SetAlpha(double current_tracking_z);
|
||||
|
||||
Q_SIGNALS:
|
||||
// RPC request succeeded.
|
||||
void RequestSucceeded();
|
||||
|
||||
private Q_SLOTS:
|
||||
// Callback when an rpc request succeeded.
|
||||
void OnRequestSuccess();
|
||||
|
||||
private:
|
||||
void QuerySubmap(int submap_id, int trajectory_id,
|
||||
ros::ServiceClient* client);
|
||||
void OnRequestFailure();
|
||||
void UpdateSceneNode();
|
||||
float UpdateAlpha(float target_alpha);
|
||||
|
||||
const int submap_id_;
|
||||
const int trajectory_id_;
|
||||
|
||||
::cartographer::common::Mutex mutex_;
|
||||
::rviz::FrameManager* frame_manager_;
|
||||
Ogre::SceneNode* const scene_node_;
|
||||
Ogre::ManualObject* manual_object_;
|
||||
Ogre::TexturePtr texture_;
|
||||
Ogre::MaterialPtr material_;
|
||||
Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_);
|
||||
geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_);
|
||||
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
|
||||
bool query_in_progress_ GUARDED_BY(mutex_);
|
||||
float resolution_ GUARDED_BY(mutex_);
|
||||
int width_ GUARDED_BY(mutex_);
|
||||
int height_ GUARDED_BY(mutex_);
|
||||
int version_ GUARDED_BY(mutex_);
|
||||
double slice_height_ GUARDED_BY(mutex_);
|
||||
double last_query_slice_height_ GUARDED_BY(mutex_);
|
||||
std::future<void> rpc_request_future_;
|
||||
std::string cells_ GUARDED_BY(mutex_);
|
||||
std::unique_ptr<::google_cartographer_msgs::SubmapQuery::Response> response_
|
||||
GUARDED_BY(mutex_);
|
||||
int texture_count_;
|
||||
float current_alpha_;
|
||||
};
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_
|
||||
|
|
@ -0,0 +1,261 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "msg_conversion.h"
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "cartographer/transform/proto/transform.pb.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "geometry_msgs/Pose.h"
|
||||
#include "geometry_msgs/Quaternion.h"
|
||||
#include "geometry_msgs/Transform.h"
|
||||
#include "geometry_msgs/TransformStamped.h"
|
||||
#include "geometry_msgs/Vector3.h"
|
||||
#include "glog/logging.h"
|
||||
#include "ros/ros.h"
|
||||
#include "ros/serialization.h"
|
||||
#include "sensor_msgs/Imu.h"
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "sensor_msgs/MultiEchoLaserScan.h"
|
||||
#include "sensor_msgs/PointCloud2.h"
|
||||
|
||||
#include "time_conversion.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace {
|
||||
|
||||
// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
|
||||
// point. The last one must be this value or RViz is not showing the point cloud
|
||||
// properly.
|
||||
constexpr float kPointCloudComponentFourMagic = 1.;
|
||||
|
||||
void ToMessage(const ::cartographer::transform::proto::Vector3d& proto,
|
||||
geometry_msgs::Vector3* vector) {
|
||||
vector->x = proto.x();
|
||||
vector->y = proto.y();
|
||||
vector->z = proto.z();
|
||||
}
|
||||
|
||||
void ToMessage(const ::cartographer::transform::proto::Quaterniond& proto,
|
||||
geometry_msgs::Quaternion* quaternion) {
|
||||
quaternion->w = proto.w();
|
||||
quaternion->x = proto.x();
|
||||
quaternion->y = proto.y();
|
||||
quaternion->z = proto.z();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
|
||||
const int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
|
||||
sensor_msgs::MultiEchoLaserScan msg;
|
||||
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||
msg.header.frame_id = frame_id;
|
||||
|
||||
msg.angle_min = laser_scan.angle_min();
|
||||
msg.angle_max = laser_scan.angle_max();
|
||||
msg.angle_increment = laser_scan.angle_increment();
|
||||
msg.time_increment = laser_scan.time_increment();
|
||||
msg.scan_time = laser_scan.scan_time();
|
||||
msg.range_min = laser_scan.range_min();
|
||||
msg.range_max = laser_scan.range_max();
|
||||
|
||||
for (const auto& echoes : laser_scan.range()) {
|
||||
msg.ranges.emplace_back();
|
||||
std::copy(echoes.value().begin(), echoes.value().end(),
|
||||
std::back_inserter(msg.ranges.back().echoes));
|
||||
}
|
||||
|
||||
for (const auto& echoes : laser_scan.intensity()) {
|
||||
msg.intensities.emplace_back();
|
||||
std::copy(echoes.value().begin(), echoes.value().end(),
|
||||
std::back_inserter(msg.intensities.back().echoes));
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
sensor_msgs::Imu ToImuMessage(const int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::Imu& imu) {
|
||||
sensor_msgs::Imu message;
|
||||
message.header.stamp =
|
||||
ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||
message.header.frame_id = frame_id;
|
||||
|
||||
ToMessage(imu.orientation(), &message.orientation);
|
||||
message.orientation_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
|
||||
ToMessage(imu.angular_velocity(), &message.angular_velocity);
|
||||
message.angular_velocity_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
|
||||
ToMessage(imu.linear_acceleration(), &message.linear_acceleration);
|
||||
message.linear_acceleration_covariance = {
|
||||
{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
|
||||
return message;
|
||||
}
|
||||
|
||||
sensor_msgs::LaserScan ToLaserScan(
|
||||
const int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
|
||||
sensor_msgs::LaserScan msg;
|
||||
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||
msg.header.frame_id = frame_id;
|
||||
|
||||
msg.angle_min = laser_scan.angle_min();
|
||||
msg.angle_max = laser_scan.angle_max();
|
||||
msg.angle_increment = laser_scan.angle_increment();
|
||||
msg.time_increment = laser_scan.time_increment();
|
||||
msg.scan_time = laser_scan.scan_time();
|
||||
msg.range_min = laser_scan.range_min();
|
||||
msg.range_max = laser_scan.range_max();
|
||||
|
||||
for (const auto& echoes : laser_scan.range()) {
|
||||
if (echoes.value_size() > 0) {
|
||||
msg.ranges.push_back(echoes.value(0));
|
||||
} else {
|
||||
msg.ranges.push_back(0.);
|
||||
}
|
||||
}
|
||||
|
||||
bool has_intensities = false;
|
||||
for (const auto& echoes : laser_scan.intensity()) {
|
||||
if (echoes.value_size() > 0) {
|
||||
msg.intensities.push_back(echoes.value(0));
|
||||
has_intensities = true;
|
||||
} else {
|
||||
msg.intensities.push_back(0);
|
||||
}
|
||||
}
|
||||
if (!has_intensities) {
|
||||
msg.intensities.clear();
|
||||
}
|
||||
|
||||
return msg;
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||
const int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) {
|
||||
CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0)
|
||||
<< "Trying to convert a laser_scan_3d that is not at the origin.";
|
||||
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||
msg.header.frame_id = frame_id;
|
||||
|
||||
const auto& point_cloud = laser_scan_3d.point_cloud();
|
||||
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
|
||||
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
|
||||
const auto num_points = point_cloud.x_size();
|
||||
|
||||
msg.height = 1;
|
||||
msg.width = num_points;
|
||||
msg.fields.resize(3);
|
||||
msg.fields[0].name = "x";
|
||||
msg.fields[0].offset = 0;
|
||||
msg.fields[0].datatype = 7;
|
||||
msg.fields[0].count = 1;
|
||||
msg.fields[1].name = "y";
|
||||
msg.fields[1].offset = 4;
|
||||
msg.fields[1].datatype = 7;
|
||||
msg.fields[1].count = 1;
|
||||
msg.fields[2].name = "z";
|
||||
msg.fields[2].offset = 8;
|
||||
msg.fields[2].datatype = 7;
|
||||
msg.fields[2].count = 1;
|
||||
msg.is_bigendian = false;
|
||||
msg.point_step = 16;
|
||||
msg.row_step = 16 * msg.width;
|
||||
msg.is_dense = true;
|
||||
msg.data.resize(16 * num_points);
|
||||
|
||||
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
||||
for (int i = 0; i < num_points; ++i) {
|
||||
stream.next(point_cloud.x(i));
|
||||
stream.next(point_cloud.y(i));
|
||||
stream.next(point_cloud.z(i));
|
||||
stream.next(kPointCloudComponentFourMagic);
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg) {
|
||||
::cartographer::sensor::proto::Imu proto;
|
||||
|
||||
if (msg.orientation_covariance[0] != -1) {
|
||||
auto* orientation = proto.mutable_orientation();
|
||||
orientation->set_x(msg.orientation.x);
|
||||
orientation->set_y(msg.orientation.y);
|
||||
orientation->set_z(msg.orientation.z);
|
||||
orientation->set_w(msg.orientation.w);
|
||||
}
|
||||
|
||||
if (msg.angular_velocity_covariance[0] != -1) {
|
||||
auto* angular_velocity = proto.mutable_angular_velocity();
|
||||
angular_velocity->set_x(msg.angular_velocity.x);
|
||||
angular_velocity->set_y(msg.angular_velocity.y);
|
||||
angular_velocity->set_z(msg.angular_velocity.z);
|
||||
}
|
||||
|
||||
if (msg.linear_acceleration_covariance[0] != -1) {
|
||||
auto* linear_acceleration = proto.mutable_linear_acceleration();
|
||||
linear_acceleration->set_x(msg.linear_acceleration.x);
|
||||
linear_acceleration->set_y(msg.linear_acceleration.y);
|
||||
linear_acceleration->set_z(msg.linear_acceleration.z);
|
||||
}
|
||||
return proto;
|
||||
}
|
||||
|
||||
::cartographer::sensor::proto::LaserScan ToCartographer(
|
||||
const sensor_msgs::LaserScan& msg) {
|
||||
::cartographer::sensor::proto::LaserScan proto;
|
||||
proto.set_angle_min(msg.angle_min);
|
||||
proto.set_angle_max(msg.angle_max);
|
||||
proto.set_angle_increment(msg.angle_increment);
|
||||
proto.set_time_increment(msg.time_increment);
|
||||
proto.set_scan_time(msg.scan_time);
|
||||
proto.set_range_min(msg.range_min);
|
||||
proto.set_range_max(msg.range_max);
|
||||
|
||||
for (const auto& range : msg.ranges) {
|
||||
proto.add_range()->mutable_value()->Add(range);
|
||||
}
|
||||
|
||||
for (const auto& intensity : msg.intensities) {
|
||||
proto.add_intensity()->mutable_value()->Add(intensity);
|
||||
}
|
||||
return proto;
|
||||
}
|
||||
|
||||
::cartographer::sensor::proto::LaserFan3D ToCartographer(
|
||||
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
|
||||
::cartographer::sensor::proto::LaserFan3D proto;
|
||||
|
||||
auto* origin = proto.mutable_origin();
|
||||
origin->set_x(0.);
|
||||
origin->set_y(0.);
|
||||
origin->set_z(0.);
|
||||
|
||||
auto* point_cloud = proto.mutable_point_cloud();
|
||||
for (const auto& point : pcl_points) {
|
||||
point_cloud->add_x(point.x);
|
||||
point_cloud->add_y(point.y);
|
||||
point_cloud->add_z(point.z);
|
||||
}
|
||||
return proto;
|
||||
}
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "pcl/point_cloud.h"
|
||||
#include "pcl/point_types.h"
|
||||
#include "sensor_msgs/Imu.h"
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "sensor_msgs/MultiEchoLaserScan.h"
|
||||
#include "sensor_msgs/PointCloud2.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
// Only uses first echo of each beam.
|
||||
sensor_msgs::LaserScan ToLaserScan(
|
||||
int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserScan& laser_scan);
|
||||
|
||||
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
|
||||
int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserScan& laser_scan);
|
||||
|
||||
sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::Imu& imu);
|
||||
|
||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||
int64 timestamp, const string& frame_id,
|
||||
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d);
|
||||
|
||||
::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg);
|
||||
|
||||
::cartographer::sensor::proto::LaserScan ToCartographer(
|
||||
const sensor_msgs::LaserScan& msg);
|
||||
|
||||
::cartographer::sensor::proto::LaserFan3D ToCartographer(
|
||||
const pcl::PointCloud<pcl::PointXYZ>& pcl_points);
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
// The topic that the node will subscrbe under.
|
||||
constexpr char kSubmapListTopic[] = "submap_list";
|
||||
|
||||
// The service we serve in the Node and query in the RViz plugin for submap
|
||||
// which are used for visualization.
|
||||
constexpr char kSubmapQueryServiceName[] = "submap_query";
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_
|
||||
|
|
@ -0,0 +1,331 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "submaps_display.h"
|
||||
|
||||
#include <OgreColourValue.h>
|
||||
#include <OgreHardwarePixelBuffer.h>
|
||||
#include <OgreManualObject.h>
|
||||
#include <OgreMaterialManager.h>
|
||||
#include <OgreOverlay.h>
|
||||
#include <OgreOverlayContainer.h>
|
||||
#include <OgreOverlayManager.h>
|
||||
#include <OgreRenderTexture.h>
|
||||
#include <OgreResourceGroupManager.h>
|
||||
#include <OgreRoot.h>
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreSceneNode.h>
|
||||
#include <OgreSharedPtr.h>
|
||||
#include <OgreTechnique.h>
|
||||
#include <OgreTexture.h>
|
||||
#include <OgreTextureManager.h>
|
||||
#include <OgreViewport.h>
|
||||
#include <cartographer/common/mutex.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <google_cartographer_msgs/SubmapList.h>
|
||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rviz/display_context.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
#include <rviz/properties/ros_topic_property.h>
|
||||
#include <rviz/properties/string_property.h>
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kMaxOnGoingRequests = 6;
|
||||
constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
|
||||
constexpr char kGlsl120Directory[] = "/glsl120";
|
||||
constexpr char kScriptsDirectory[] = "/scripts";
|
||||
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
|
||||
constexpr char kScreenBlitSourceMaterialName[] =
|
||||
"google_cartographer/ScreenBlit";
|
||||
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
|
||||
constexpr char kMapTextureName[] = "MapTexture";
|
||||
constexpr char kMapOverlayName[] = "MapOverlay";
|
||||
constexpr char kSubmapsSceneCameraName[] = "SubmapsSceneCamera";
|
||||
constexpr char kSubmapTexturesGroup[] = "SubmapTexturesGroup";
|
||||
constexpr char kDefaultMapFrame[] = "map";
|
||||
constexpr char kDefaultTrackingFrame[] = "base_link";
|
||||
|
||||
} // namespace
|
||||
|
||||
SubmapsDisplay::SubmapsDisplay()
|
||||
: Display(),
|
||||
rtt_count_(0),
|
||||
scene_manager_listener_([this]() { UpdateMapTexture(); }),
|
||||
tf_listener_(tf_buffer_) {
|
||||
connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
|
||||
topic_property_ = new ::rviz::RosTopicProperty(
|
||||
"Topic", "",
|
||||
QString::fromStdString(ros::message_traits::datatype<
|
||||
::google_cartographer_msgs::SubmapList>()),
|
||||
"google_cartographer_msgs::SubmapList topic to subscribe to.", this,
|
||||
SLOT(UpdateTopic()));
|
||||
submap_query_service_property_ = new ::rviz::StringProperty(
|
||||
"Submap query service", "", "Submap query service to connect to.", this,
|
||||
SLOT(UpdateSubmapQueryServiceName()));
|
||||
map_frame_property_ = new ::rviz::StringProperty(
|
||||
"Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
|
||||
this);
|
||||
tracking_frame_property_ = new ::rviz::StringProperty(
|
||||
"Tracking frame", kDefaultTrackingFrame,
|
||||
"Tracking frame, used for fading out submaps.", this);
|
||||
client_ =
|
||||
update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>("");
|
||||
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem",
|
||||
ROS_PACKAGE_NAME);
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem",
|
||||
ROS_PACKAGE_NAME);
|
||||
Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
|
||||
}
|
||||
|
||||
SubmapsDisplay::~SubmapsDisplay() {
|
||||
Unsubscribe();
|
||||
client_.shutdown();
|
||||
Clear();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::onInitialize() {
|
||||
submaps_scene_manager_ =
|
||||
Ogre::Root::getSingletonPtr()->createSceneManager(Ogre::ST_GENERIC);
|
||||
submaps_scene_camera_ =
|
||||
submaps_scene_manager_->createCamera(kSubmapsSceneCameraName);
|
||||
submap_scene_material_ = Ogre::MaterialManager::getSingleton().create(
|
||||
kMapTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
|
||||
|
||||
screen_blit_material_ = Ogre::MaterialManager::getSingleton().getByName(
|
||||
kScreenBlitSourceMaterialName);
|
||||
screen_blit_material_ = screen_blit_material_->clone(kScreenBlitMaterialName);
|
||||
screen_blit_material_->setReceiveShadows(false);
|
||||
screen_blit_material_->getTechnique(0)->setLightingEnabled(false);
|
||||
screen_blit_material_->setDepthWriteEnabled(false);
|
||||
|
||||
Ogre::OverlayManager& overlay_manager = Ogre::OverlayManager::getSingleton();
|
||||
overlay_ = overlay_manager.create(kMapOverlayName);
|
||||
panel_ = static_cast<Ogre::OverlayContainer*>(
|
||||
overlay_manager.createOverlayElement("Panel", "PanelName"));
|
||||
overlay_->add2D(panel_);
|
||||
panel_->setPosition(0.0, 0.0);
|
||||
panel_->setDimensions(1., 1.);
|
||||
panel_->setMaterialName(kScreenBlitMaterialName);
|
||||
|
||||
Ogre::ResourceGroupManager::getSingleton().createResourceGroup(
|
||||
kSubmapTexturesGroup);
|
||||
|
||||
scene_manager_->addListener(&scene_manager_listener_);
|
||||
UpdateTopic();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::UpdateTopic() {
|
||||
Unsubscribe();
|
||||
Clear();
|
||||
Subscribe();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::UpdateSubmapQueryServiceName() {
|
||||
Unsubscribe();
|
||||
Clear();
|
||||
client_.shutdown();
|
||||
client_ = update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>(
|
||||
submap_query_service_property_->getStdString());
|
||||
Subscribe();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::reset() {
|
||||
Display::reset();
|
||||
|
||||
Clear();
|
||||
UpdateTopic();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::onEnable() { Subscribe(); }
|
||||
|
||||
void SubmapsDisplay::onDisable() {
|
||||
Unsubscribe();
|
||||
Clear();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::Subscribe() {
|
||||
if (!isEnabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!topic_property_->getTopic().isEmpty()) {
|
||||
try {
|
||||
submap_list_subscriber_ =
|
||||
update_nh_.subscribe(topic_property_->getTopicStd(), 1,
|
||||
&SubmapsDisplay::IncomingSubmapList, this,
|
||||
ros::TransportHints().reliable());
|
||||
setStatus(::rviz::StatusProperty::Ok, "Topic", "OK");
|
||||
} catch (ros::Exception& e) {
|
||||
setStatus(::rviz::StatusProperty::Error, "Topic",
|
||||
QString("Error subscribing: ") + e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
|
||||
|
||||
void SubmapsDisplay::IncomingSubmapList(
|
||||
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg) {
|
||||
submap_list_ = *msg;
|
||||
Q_EMIT SubmapListUpdated();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::Clear() {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
submaps_scene_manager_->clearScene();
|
||||
if (!rttTexture_.isNull()) {
|
||||
rttTexture_->unload();
|
||||
rttTexture_.setNull();
|
||||
}
|
||||
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
|
||||
kSubmapTexturesGroup);
|
||||
trajectories_.clear();
|
||||
overlay_->hide();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::RequestNewSubmaps() {
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
|
||||
++trajectory_id) {
|
||||
if (trajectory_id >= trajectories_.size()) {
|
||||
trajectories_.emplace_back(new Trajectory);
|
||||
}
|
||||
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
|
||||
submap_list_.trajectory[trajectory_id].submap;
|
||||
if (submap_entries.empty()) {
|
||||
return;
|
||||
}
|
||||
for (int submap_id = trajectories_[trajectory_id]->Size();
|
||||
submap_id < submap_entries.size(); ++submap_id) {
|
||||
trajectories_[trajectory_id]->Add(submap_id, trajectory_id,
|
||||
context_->getFrameManager(),
|
||||
submaps_scene_manager_);
|
||||
}
|
||||
}
|
||||
int num_ongoing_requests = 0;
|
||||
for (const auto& trajectory : trajectories_) {
|
||||
for (int i = 0; i < trajectory->Size(); ++i) {
|
||||
if (trajectory->Get(i).QueryInProgress()) {
|
||||
++num_ongoing_requests;
|
||||
if (num_ongoing_requests == kMaxOnGoingRequests) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
|
||||
++trajectory_id) {
|
||||
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
|
||||
submap_list_.trajectory[trajectory_id].submap;
|
||||
for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
|
||||
--submap_id) {
|
||||
if (trajectories_[trajectory_id]->Get(submap_id).Update(
|
||||
submap_entries[submap_id], &client_)) {
|
||||
++num_ongoing_requests;
|
||||
if (num_ongoing_requests == kMaxOnGoingRequests) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::UpdateMapTexture() {
|
||||
if (trajectories_.empty()) {
|
||||
return;
|
||||
}
|
||||
const int width = scene_manager_->getCurrentViewport()->getActualWidth();
|
||||
const int height = scene_manager_->getCurrentViewport()->getActualHeight();
|
||||
if (!rttTexture_.isNull()) {
|
||||
rttTexture_->unload();
|
||||
rttTexture_.setNull();
|
||||
}
|
||||
// If the rtt texture is freed every time UpdateMapTexture() is called, the
|
||||
// code slows down a lot. Therefore, we assign them to a group and free them
|
||||
// every 100th texture.
|
||||
if (rtt_count_ % 100 == 0) {
|
||||
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
|
||||
kSubmapTexturesGroup);
|
||||
}
|
||||
rttTexture_ =
|
||||
Ogre::Root::getSingletonPtr()->getTextureManager()->createManual(
|
||||
kSubmapsRttPrefix + std::to_string(rtt_count_), kSubmapTexturesGroup,
|
||||
Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_RG8,
|
||||
Ogre::TU_RENDERTARGET);
|
||||
rtt_count_++;
|
||||
|
||||
Ogre::Pass* rtt_pass = submap_scene_material_->getTechnique(0)->getPass(0);
|
||||
Ogre::TextureUnitState* const rtt_tex_unit =
|
||||
rtt_pass->getNumTextureUnitStates() > 0
|
||||
? rtt_pass->getTextureUnitState(0)
|
||||
: rtt_pass->createTextureUnitState();
|
||||
rtt_tex_unit->setTexture(rttTexture_);
|
||||
|
||||
Ogre::RenderTexture* const renderTexture =
|
||||
rttTexture_->getBuffer()->getRenderTarget();
|
||||
renderTexture->addViewport(submaps_scene_camera_)
|
||||
->setBackgroundColour(Ogre::ColourValue(0.5f, 0.f, 0.f));
|
||||
{
|
||||
::cartographer::common::MutexLocker locker(&mutex_);
|
||||
// TODO(pedrofernandez): Add support for more than one trajectory.
|
||||
for (int i = 0; i < trajectories_.front()->Size(); ++i) {
|
||||
trajectories_.front()->Get(i).Transform(ros::Time());
|
||||
try {
|
||||
const ::geometry_msgs::TransformStamped transform_stamped =
|
||||
tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
|
||||
tracking_frame_property_->getStdString(),
|
||||
ros::Time(0) /* latest */);
|
||||
trajectories_.front()->Get(i).SetAlpha(
|
||||
transform_stamped.transform.translation.z);
|
||||
} catch (tf2::TransformException& e) {
|
||||
ROS_WARN("Could not compute submap fading: %s", e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
Ogre::Camera* const actual_camera =
|
||||
scene_manager_->getCurrentViewport()->getCamera();
|
||||
submaps_scene_camera_->synchroniseBaseSettingsWith(actual_camera);
|
||||
submaps_scene_camera_->setCustomProjectionMatrix(
|
||||
true, actual_camera->getProjectionMatrix());
|
||||
renderTexture->update();
|
||||
|
||||
Ogre::Pass* const pass = screen_blit_material_->getTechnique(0)->getPass(0);
|
||||
pass->setSceneBlending(Ogre::SBF_SOURCE_ALPHA,
|
||||
Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
|
||||
Ogre::TextureUnitState* const tex_unit = pass->getNumTextureUnitStates() > 0
|
||||
? pass->getTextureUnitState(0)
|
||||
: pass->createTextureUnitState();
|
||||
tex_unit->setTextureName(rttTexture_->getName());
|
||||
tex_unit->setTextureFiltering(Ogre::TFO_NONE);
|
||||
overlay_->show();
|
||||
}
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(cartographer_ros::rviz::SubmapsDisplay, ::rviz::Display)
|
||||
|
|
@ -0,0 +1,127 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
|
||||
|
||||
#include <OgreMaterial.h>
|
||||
#include <OgreOverlay.h>
|
||||
#include <OgreOverlayContainer.h>
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreSharedPtr.h>
|
||||
#include <OgreTexture.h>
|
||||
#include <OgreVector3.h>
|
||||
#include <cartographer/common/mutex.h>
|
||||
#include <cartographer/common/port.h>
|
||||
#include <google_cartographer_msgs/SubmapList.h>
|
||||
#include <nav_msgs/MapMetaData.h>
|
||||
#include <ros/time.h>
|
||||
#include <rviz/display.h>
|
||||
#include <rviz/properties/ros_topic_property.h>
|
||||
#include <tf/tfMessage.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "drawable_submap.h"
|
||||
#include "trajectory.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
// RViz plugin used for displaying maps which are represented by a collection of
|
||||
// submaps.
|
||||
//
|
||||
// We keep a separate Ogre scene to the one provided by rviz and in it we place
|
||||
// every submap as a SceneNode. We show an X-ray view of the map which is
|
||||
// achieved by shipping textures for every submap containing pre-multiplied
|
||||
// alpha and grayscale values, these are then alpha blended together onto an
|
||||
// offscreen texture. This offscreen texture is then screen blit onto the screen
|
||||
// as a grayscale image.
|
||||
class SubmapsDisplay : public ::rviz::Display {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
SubmapsDisplay();
|
||||
~SubmapsDisplay() override;
|
||||
|
||||
SubmapsDisplay(const SubmapsDisplay&) = delete;
|
||||
SubmapsDisplay& operator=(const SubmapsDisplay&) = delete;
|
||||
|
||||
// Called by RViz on initialization of the plugin.
|
||||
void onInitialize() override;
|
||||
// Called to tell the display to clear its state.
|
||||
void reset() override;
|
||||
|
||||
Q_SIGNALS:
|
||||
void SubmapListUpdated();
|
||||
|
||||
private Q_SLOTS:
|
||||
void RequestNewSubmaps();
|
||||
void UpdateTopic();
|
||||
void UpdateSubmapQueryServiceName();
|
||||
|
||||
private:
|
||||
class SceneManagerListener : public Ogre::SceneManager::Listener {
|
||||
public:
|
||||
SceneManagerListener(std::function<void()> callback)
|
||||
: callback_(callback) {}
|
||||
void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) {
|
||||
callback_();
|
||||
}
|
||||
|
||||
private:
|
||||
std::function<void()> callback_;
|
||||
};
|
||||
|
||||
void onEnable() override;
|
||||
void onDisable() override;
|
||||
void Subscribe();
|
||||
void Unsubscribe();
|
||||
void UpdateMapTexture();
|
||||
void IncomingSubmapList(
|
||||
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg);
|
||||
// Clears the current map.
|
||||
void Clear();
|
||||
void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
|
||||
|
||||
int rtt_count_;
|
||||
SceneManagerListener scene_manager_listener_;
|
||||
::google_cartographer_msgs::SubmapList submap_list_;
|
||||
ros::Subscriber submap_list_subscriber_;
|
||||
::tf2_ros::Buffer tf_buffer_;
|
||||
::tf2_ros::TransformListener tf_listener_;
|
||||
ros::ServiceClient client_;
|
||||
::rviz::RosTopicProperty* topic_property_;
|
||||
::rviz::StringProperty* submap_query_service_property_;
|
||||
::rviz::StringProperty* map_frame_property_;
|
||||
::rviz::StringProperty* tracking_frame_property_;
|
||||
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
|
||||
Ogre::SceneManager* submaps_scene_manager_;
|
||||
Ogre::Camera* submaps_scene_camera_;
|
||||
Ogre::MaterialPtr submap_scene_material_;
|
||||
Ogre::MaterialPtr screen_blit_material_;
|
||||
Ogre::Overlay* overlay_;
|
||||
Ogre::OverlayContainer* panel_;
|
||||
Ogre::TexturePtr rttTexture_;
|
||||
::cartographer::common::Mutex mutex_;
|
||||
};
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "time_conversion.h"
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "ros/ros.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
::ros::Time ToRos(::cartographer::common::Time time) {
|
||||
int64 uts_timestamp = ::cartographer::common::ToUniversal(time);
|
||||
int64 ns_since_unix_epoch =
|
||||
(uts_timestamp - kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) *
|
||||
100ll;
|
||||
::ros::Time ros_time;
|
||||
ros_time.fromNSec(ns_since_unix_epoch);
|
||||
return ros_time;
|
||||
}
|
||||
|
||||
// TODO(pedrofernandez): Write test.
|
||||
::cartographer::common::Time FromRos(const ::ros::Time& time) {
|
||||
// The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
|
||||
// exactly 719162 days before the Unix epoch.
|
||||
return ::cartographer::common::FromUniversal(
|
||||
(time.sec + kUtsEpochOffsetFromUnixEpochInSeconds) * 10000000ll +
|
||||
(time.nsec + 50) / 100); // + 50 to get the rounding correct.
|
||||
}
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "ros/ros.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds =
|
||||
(719162ll * 24ll * 60ll * 60ll);
|
||||
|
||||
::ros::Time ToRos(::cartographer::common::Time time);
|
||||
|
||||
::cartographer::common::Time FromRos(const ::ros::Time& time);
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "time_conversion.h"
|
||||
|
||||
#include <chrono>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "ros/ros.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
namespace {
|
||||
|
||||
TEST(TimeConversion, testToRos) {
|
||||
std::vector<int64> values = {0, 1469091375, 1466481821, 1462101382,
|
||||
1468238899};
|
||||
for (int64 seconds_since_epoch : values) {
|
||||
::ros::Time ros_now;
|
||||
ros_now.fromSec(seconds_since_epoch);
|
||||
::cartographer::common::Time cartographer_now(
|
||||
::cartographer::common::FromSeconds(
|
||||
seconds_since_epoch + kUtsEpochOffsetFromUnixEpochInSeconds));
|
||||
EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now));
|
||||
EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "trajectory.h"
|
||||
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreSceneNode.h>
|
||||
#include <cartographer/common/make_unique.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
#include "drawable_submap.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
void Trajectory::Add(const int submap_id, const int trajectory_id,
|
||||
::rviz::FrameManager* const frame_manager,
|
||||
Ogre::SceneManager* const scene_manager) {
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
drawable_submaps_.push_back(
|
||||
::cartographer::common::make_unique<DrawableSubmap>(
|
||||
submap_id, trajectory_id, frame_manager, scene_manager));
|
||||
}
|
||||
|
||||
DrawableSubmap& Trajectory::Get(const int index) {
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
return *drawable_submaps_[index];
|
||||
}
|
||||
|
||||
int Trajectory::Size() { return drawable_submaps_.size(); }
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
|
||||
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
|
||||
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreSceneNode.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
#include "drawable_submap.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
namespace rviz {
|
||||
|
||||
// Contains a list of drawable submaps.
|
||||
class Trajectory {
|
||||
public:
|
||||
Trajectory() = default;
|
||||
|
||||
Trajectory(const Trajectory&) = delete;
|
||||
Trajectory& operator=(const Trajectory&) = delete;
|
||||
|
||||
// Creates a new DrawableSubmap and stores it as part of this trajectory.
|
||||
void Add(int submap_id, int trajectory_id,
|
||||
::rviz::FrameManager* frame_manager,
|
||||
Ogre::SceneManager* scene_manager);
|
||||
// Gets the submap at 'index' and returns a non-const reference.
|
||||
DrawableSubmap& Get(int index);
|
||||
// Returns the number of DrawableSubmaps this trajectory contains.
|
||||
int Size();
|
||||
|
||||
private:
|
||||
std::mutex mutex_;
|
||||
std::vector<std::unique_ptr<DrawableSubmap>> drawable_submaps_;
|
||||
};
|
||||
|
||||
} // namespace rviz
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<robot name="google_cartographer">
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.2 1"/>
|
||||
</material>
|
||||
<material name="gray">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.2 0.4 0.2 1"/>
|
||||
</material>
|
||||
|
||||
<link name="/imu_link">
|
||||
<visual>
|
||||
<origin xyz="0.01 0.01 -0.01"/>
|
||||
<geometry>
|
||||
<box size="0.055 0.055 0.02"/>
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/horizontal_laser_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 -0.01"/>
|
||||
<geometry>
|
||||
<cylinder length="0.09" radius="0.03"/>
|
||||
</geometry>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/vertical_laser_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 -0.01"/>
|
||||
<geometry>
|
||||
<cylinder length="0.09" radius="0.03"/>
|
||||
</geometry>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/base_footprint">
|
||||
<visual>
|
||||
<origin xyz="0.18 0.0 0.2"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.4 0.8"/>
|
||||
</geometry>
|
||||
<material name="green"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_joint" type="fixed">
|
||||
<child link="/imu_link"/>
|
||||
<parent link="/base_footprint"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
|
||||
</joint>
|
||||
|
||||
<joint name="horizontal_laser_link_joint" type="fixed">
|
||||
<parent link="/imu_link"/>
|
||||
<child link="/horizontal_laser_link"/>
|
||||
<origin xyz="0.1251 0.0937 0.05262"/>
|
||||
</joint>
|
||||
|
||||
<joint name="vertical_laser_link_joint" type="fixed">
|
||||
<parent link="/imu_link"/>
|
||||
<child link="/vertical_laser_link"/>
|
||||
<origin rpy="0.0 -1.57 3.14" xyz="0.1251 0.0937 0.17772"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<robot name="google_cartographer">
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.2 1"/>
|
||||
</material>
|
||||
<material name="gray">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.2 0.4 0.2 1"/>
|
||||
</material>
|
||||
|
||||
<link name="/imu_link">
|
||||
<visual>
|
||||
<origin xyz="0.01 0.01 -0.01"/>
|
||||
<geometry>
|
||||
<box size="0.055 0.055 0.02"/>
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/horizontal_vlp16_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 -0.01"/>
|
||||
<geometry>
|
||||
<cylinder length="0.09" radius="0.03"/>
|
||||
</geometry>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/vertical_vlp16_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 -0.01"/>
|
||||
<geometry>
|
||||
<cylinder length="0.09" radius="0.03"/>
|
||||
</geometry>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/base_link">
|
||||
<visual>
|
||||
<origin xyz="0.18 0.0 0.2"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.4 0.8"/>
|
||||
</geometry>
|
||||
<material name="green"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_joint" type="fixed">
|
||||
<parent link="/base_link"/>
|
||||
<child link="/imu_link"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
|
||||
</joint>
|
||||
|
||||
<joint name="horizontal_vlp16_link_joint" type="fixed">
|
||||
<parent link="/base_link"/>
|
||||
<child link="/horizontal_vlp16_link"/>
|
||||
<origin xyz="0.01 0. 0.19" rpy="0. -0.1745 3.1416" />
|
||||
</joint>
|
||||
|
||||
<joint name="vertical_vlp16_link_joint" type="fixed">
|
||||
<parent link="/base_link"/>
|
||||
<child link="/vertical_vlp16_link"/>
|
||||
<origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0."/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<robot name="google_cartographer">
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.2 1"/>
|
||||
</material>
|
||||
<material name="gray">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.2 0.4 0.2 1"/>
|
||||
</material>
|
||||
|
||||
<link name="/horizontal_laser_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 0.0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.09" radius="0.03"/>
|
||||
</geometry>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="/base_link">
|
||||
<visual>
|
||||
<origin xyz="0. 0. 0."/>
|
||||
<geometry>
|
||||
<box size="0.03 0.4 0.8"/>
|
||||
</geometry>
|
||||
<material name="green"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="horizontal_laser_link_joint" type="fixed">
|
||||
<parent link="/base_link"/>
|
||||
<child link="/horizontal_laser_link"/>
|
||||
<origin xyz="0. 0. 0.6"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(google_cartographer_msgs)
|
||||
set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
|
||||
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
SubmapList.msg
|
||||
TrajectorySubmapList.msg
|
||||
SubmapEntry.msg
|
||||
)
|
||||
|
||||
add_service_files(
|
||||
FILES
|
||||
SubmapQuery.srv
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
uint32 submap_version
|
||||
geometry_msgs/Pose pose
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
TrajectorySubmapList[] trajectory
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
SubmapEntry[] submap
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<package>
|
||||
<name>google_cartographer_msgs</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
The google_cartographer_msgs package.
|
||||
</description>
|
||||
<maintainer email="nobody@google.com">Google</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
# Copyright 2016 The Cartographer Authors
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
int32 submap_id
|
||||
int32 trajectory_id
|
||||
---
|
||||
int32 submap_id
|
||||
int32 submap_version
|
||||
uint8[] cells
|
||||
int32 width
|
||||
int32 height
|
||||
float64 resolution
|
||||
geometry_msgs/Pose slice_pose
|
||||
string error_message
|
||||
Loading…
Reference in New Issue