commit 63c2321d262c6ab7917e6027022d4bf7308d4884 Author: Damon Kohler Date: Wed Aug 3 12:45:08 2016 +0200 Initial import of Cartographer codebase. diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..663aac4 --- /dev/null +++ b/AUTHORS @@ -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 diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..2827b7d --- /dev/null +++ b/CONTRIBUTING.md @@ -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). diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -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. diff --git a/google_cartographer/CMakeLists.txt b/google_cartographer/CMakeLists.txt new file mode 100644 index 0000000..77d8088 --- /dev/null +++ b/google_cartographer/CMakeLists.txt @@ -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} +) diff --git a/google_cartographer/configuration_files/3d_mapping.lua b/google_cartographer/configuration_files/3d_mapping.lua new file mode 100644 index 0000000..1d42b17 --- /dev/null +++ b/google_cartographer/configuration_files/3d_mapping.lua @@ -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 diff --git a/google_cartographer/configuration_files/backpack_3d.lua b/google_cartographer/configuration_files/backpack_3d.lua new file mode 100644 index 0000000..43a4a4c --- /dev/null +++ b/google_cartographer/configuration_files/backpack_3d.lua @@ -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 diff --git a/google_cartographer/configuration_files/turtlebot.lua b/google_cartographer/configuration_files/turtlebot.lua new file mode 100644 index 0000000..2f1ff65 --- /dev/null +++ b/google_cartographer/configuration_files/turtlebot.lua @@ -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 diff --git a/google_cartographer/launch/3d_mapping.launch b/google_cartographer/launch/3d_mapping.launch new file mode 100644 index 0000000..2671e2b --- /dev/null +++ b/google_cartographer/launch/3d_mapping.launch @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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. + + + + diff --git a/google_cartographer/launch/state.launch b/google_cartographer/launch/state.launch new file mode 100644 index 0000000..c07f760 --- /dev/null +++ b/google_cartographer/launch/state.launch @@ -0,0 +1,73 @@ + + + + + + + + + + # 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"] + + + + + diff --git a/google_cartographer/launch/turtlebot.launch b/google_cartographer/launch/turtlebot.launch new file mode 100644 index 0000000..75c9576 --- /dev/null +++ b/google_cartographer/launch/turtlebot.launch @@ -0,0 +1,41 @@ + + + + + + + + + + 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. + + + + diff --git a/google_cartographer/ogre_media/materials/glsl120/glsl120.program b/google_cartographer/ogre_media/materials/glsl120/glsl120.program new file mode 100644 index 0000000..045f381 --- /dev/null +++ b/google_cartographer/ogre_media/materials/glsl120/glsl120.program @@ -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 +} diff --git a/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag b/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag new file mode 100644 index 0000000..195d813 --- /dev/null +++ b/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag @@ -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); +} diff --git a/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert b/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert new file mode 100644 index 0000000..74d1bba --- /dev/null +++ b/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert @@ -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; +} diff --git a/google_cartographer/ogre_media/materials/glsl120/submap.frag b/google_cartographer/ogre_media/materials/glsl120/submap.frag new file mode 100644 index 0000000..1998f53 --- /dev/null +++ b/google_cartographer/ogre_media/materials/glsl120/submap.frag @@ -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); +} diff --git a/google_cartographer/ogre_media/materials/glsl120/submap.vert b/google_cartographer/ogre_media/materials/glsl120/submap.vert new file mode 100644 index 0000000..864bfa1 --- /dev/null +++ b/google_cartographer/ogre_media/materials/glsl120/submap.vert @@ -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(); +} diff --git a/google_cartographer/ogre_media/materials/scripts/screen_blit.material b/google_cartographer/ogre_media/materials/scripts/screen_blit.material new file mode 100644 index 0000000..cd850fd --- /dev/null +++ b/google_cartographer/ogre_media/materials/scripts/screen_blit.material @@ -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 + } + } + } +} diff --git a/google_cartographer/ogre_media/materials/scripts/submap.material b/google_cartographer/ogre_media/materials/scripts/submap.material new file mode 100644 index 0000000..1304156 --- /dev/null +++ b/google_cartographer/ogre_media/materials/scripts/submap.material @@ -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 + } + } + } +} diff --git a/google_cartographer/package.xml b/google_cartographer/package.xml new file mode 100644 index 0000000..bf3d0b9 --- /dev/null +++ b/google_cartographer/package.xml @@ -0,0 +1,68 @@ + + + + + google_cartographer + 1.0.0 + The google_cartographer package. + Google + Apache 2.0 + + catkin + + eigen_conversions + geometry_msgs + google_cartographer_msgs + libpcl-all-dev + message_generation + nav_msgs + pcl_conversions + qtbase5-dev + rosbag + roscpp + roslib + rviz + sensor_msgs + std_msgs + tf2 + tf2_eigen + visualization_msgs + + eigen_conversions + geometry_msgs + google_cartographer_msgs + libpcl-all + libqt5-core + libqt5-gui + libqt5-widgets + message_runtime + nav_msgs + pcl_conversions + rosbag + roscpp + roslib + rviz + sensor_msgs + std_msgs + tf2_eigen + tf2 + visualization_msgs + + + + + diff --git a/google_cartographer/rviz_plugin_description.xml b/google_cartographer/rviz_plugin_description.xml new file mode 100644 index 0000000..b990bd7 --- /dev/null +++ b/google_cartographer/rviz_plugin_description.xml @@ -0,0 +1,25 @@ + + + + + + Displays Google Cartographer's submaps as a unified map in RViz. + + + diff --git a/google_cartographer/src/cartographer_node_main.cc b/google_cartographer/src/cartographer_node_main.cc new file mode 100644 index 0000000..5dc619a --- /dev/null +++ b/google_cartographer/src/cartographer_node_main.cc @@ -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 +#include +#include +#include + +#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 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 + 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 sensor_collator_; + ros::NodeHandle node_handle_; + ros::Subscriber imu_subscriber_; + ros::Subscriber laser_2d_subscriber_; + std::vector 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( + 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( + 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()); + 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_points; + pcl::fromROSMsg(*msg, pcl_points); + + auto sensor_data = ::cartographer::common::make_unique( + 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())); +} + +template +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("tracking_frame"); + map_frame_ = GetParamOrDie("map_frame"); + laser_min_range_m_ = GetParamOrDie("laser_min_range_m"); + laser_max_range_m_ = GetParamOrDie("laser_max_range_m"); + laser_missing_echo_ray_length_m_ = + GetParamOrDie("laser_missing_echo_ray_length_m"); + + // Subscribe to the IMU. + const string imu_topic = GetParamOrDie("imu_topic"); + imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize, + &Node::ImuMessageCallback, this); + std::unordered_set 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("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("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>("configuration_files_directories")); + const string code = file_resolver->GetFileContentOrDie( + GetParamOrDie("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>("laser_scan_3d_topics"); + for (const auto& topic : topics) { + laser_3d_subscribers_.push_back(node_handle_.subscribe( + topic, kSubscriberQueueSize, + boost::function( + [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 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 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; +} diff --git a/google_cartographer/src/drawable_submap.cc b/google_cartographer/src/drawable_submap.cc new file mode 100644 index 0000000..5e2a035 --- /dev/null +++ b/google_cartographer/src/drawable_submap.cc @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +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::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 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 diff --git a/google_cartographer/src/drawable_submap.h b/google_cartographer/src/drawable_submap.h new file mode 100644 index 0000000..cbdd53f --- /dev/null +++ b/google_cartographer/src/drawable_submap.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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 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_ diff --git a/google_cartographer/src/msg_conversion.cc b/google_cartographer/src/msg_conversion.cc new file mode 100644 index 0000000..31a18e4 --- /dev/null +++ b/google_cartographer/src/msg_conversion.cc @@ -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_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 diff --git a/google_cartographer/src/msg_conversion.h b/google_cartographer/src/msg_conversion.h new file mode 100644 index 0000000..f553d04 --- /dev/null +++ b/google_cartographer/src/msg_conversion.h @@ -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_points); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ diff --git a/google_cartographer/src/node_constants.h b/google_cartographer/src/node_constants.h new file mode 100644 index 0000000..84c3330 --- /dev/null +++ b/google_cartographer/src/node_constants.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_ diff --git a/google_cartographer/src/submaps_display.cc b/google_cartographer/src/submaps_display.cc new file mode 100644 index 0000000..10e24f4 --- /dev/null +++ b/google_cartographer/src/submaps_display.cc @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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( + 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) diff --git a/google_cartographer/src/submaps_display.h b/google_cartographer/src/submaps_display.h new file mode 100644 index 0000000..f3e85e6 --- /dev/null +++ b/google_cartographer/src/submaps_display.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 callback) + : callback_(callback) {} + void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) { + callback_(); + } + + private: + std::function 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> 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_ diff --git a/google_cartographer/src/time_conversion.cc b/google_cartographer/src/time_conversion.cc new file mode 100644 index 0000000..7dfda18 --- /dev/null +++ b/google_cartographer/src/time_conversion.cc @@ -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 diff --git a/google_cartographer/src/time_conversion.h b/google_cartographer/src/time_conversion.h new file mode 100644 index 0000000..863d871 --- /dev/null +++ b/google_cartographer/src/time_conversion.h @@ -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_ diff --git a/google_cartographer/src/time_conversion_test.cc b/google_cartographer/src/time_conversion_test.cc new file mode 100644 index 0000000..693a67b --- /dev/null +++ b/google_cartographer/src/time_conversion_test.cc @@ -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 + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +namespace { + +TEST(TimeConversion, testToRos) { + std::vector 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 diff --git a/google_cartographer/src/trajectory.cc b/google_cartographer/src/trajectory.cc new file mode 100644 index 0000000..995a605 --- /dev/null +++ b/google_cartographer/src/trajectory.cc @@ -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 +#include +#include +#include + +#include +#include +#include + +#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 guard(mutex_); + drawable_submaps_.push_back( + ::cartographer::common::make_unique( + submap_id, trajectory_id, frame_manager, scene_manager)); +} + +DrawableSubmap& Trajectory::Get(const int index) { + std::lock_guard guard(mutex_); + return *drawable_submaps_[index]; +} + +int Trajectory::Size() { return drawable_submaps_.size(); } + +} // namespace rviz +} // namespace cartographer_ros diff --git a/google_cartographer/src/trajectory.h b/google_cartographer/src/trajectory.h new file mode 100644 index 0000000..12ff234 --- /dev/null +++ b/google_cartographer/src/trajectory.h @@ -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 +#include +#include + +#include +#include +#include + +#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> drawable_submaps_; +}; + +} // namespace rviz +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ diff --git a/google_cartographer/urdf/backpack_2d.urdf b/google_cartographer/urdf/backpack_2d.urdf new file mode 100644 index 0000000..e72be50 --- /dev/null +++ b/google_cartographer/urdf/backpack_2d.urdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/google_cartographer/urdf/backpack_3d.urdf b/google_cartographer/urdf/backpack_3d.urdf new file mode 100644 index 0000000..89f32ef --- /dev/null +++ b/google_cartographer/urdf/backpack_3d.urdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/google_cartographer/urdf/turtlebot.urdf b/google_cartographer/urdf/turtlebot.urdf new file mode 100644 index 0000000..f97dcb7 --- /dev/null +++ b/google_cartographer/urdf/turtlebot.urdf @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/google_cartographer_msgs/CMakeLists.txt b/google_cartographer_msgs/CMakeLists.txt new file mode 100644 index 0000000..9551707 --- /dev/null +++ b/google_cartographer_msgs/CMakeLists.txt @@ -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() diff --git a/google_cartographer_msgs/msg/SubmapEntry.msg b/google_cartographer_msgs/msg/SubmapEntry.msg new file mode 100644 index 0000000..605de24 --- /dev/null +++ b/google_cartographer_msgs/msg/SubmapEntry.msg @@ -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 diff --git a/google_cartographer_msgs/msg/SubmapList.msg b/google_cartographer_msgs/msg/SubmapList.msg new file mode 100644 index 0000000..dbc7b08 --- /dev/null +++ b/google_cartographer_msgs/msg/SubmapList.msg @@ -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 diff --git a/google_cartographer_msgs/msg/TrajectorySubmapList.msg b/google_cartographer_msgs/msg/TrajectorySubmapList.msg new file mode 100644 index 0000000..678a886 --- /dev/null +++ b/google_cartographer_msgs/msg/TrajectorySubmapList.msg @@ -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 diff --git a/google_cartographer_msgs/package.xml b/google_cartographer_msgs/package.xml new file mode 100644 index 0000000..a6d2cad --- /dev/null +++ b/google_cartographer_msgs/package.xml @@ -0,0 +1,29 @@ + + + + google_cartographer_msgs + 1.0.0 + + The google_cartographer_msgs package. + + Google + Apache 2.0 + + catkin + + message_generation + diff --git a/google_cartographer_msgs/srv/SubmapQuery.srv b/google_cartographer_msgs/srv/SubmapQuery.srv new file mode 100644 index 0000000..4463260 --- /dev/null +++ b/google_cartographer_msgs/srv/SubmapQuery.srv @@ -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