diff --git a/ros/google_cartographer/CMakeLists.txt b/ros/google_cartographer/CMakeLists.txt
deleted file mode 100644
index 77d8088..0000000
--- a/ros/google_cartographer/CMakeLists.txt
+++ /dev/null
@@ -1,153 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-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/ros/google_cartographer/configuration_files/3d_mapping.lua b/ros/google_cartographer/configuration_files/3d_mapping.lua
deleted file mode 100644
index 1d42b17..0000000
--- a/ros/google_cartographer/configuration_files/3d_mapping.lua
+++ /dev/null
@@ -1,19 +0,0 @@
--- Copyright 2016 The Cartographer Authors
---
--- Licensed under the Apache License, Version 2.0 (the "License");
--- you may not use this file except in compliance with the License.
--- You may obtain a copy of the License at
---
--- http://www.apache.org/licenses/LICENSE-2.0
---
--- Unless required by applicable law or agreed to in writing, software
--- distributed under the License is distributed on an "AS IS" BASIS,
--- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
--- See the License for the specific language governing permissions and
--- limitations under the License.
-
-include "backpack_3d.lua"
-
-options.trajectory_builder.scans_per_accumulation = 2
-
-return options
diff --git a/ros/google_cartographer/configuration_files/backpack_3d.lua b/ros/google_cartographer/configuration_files/backpack_3d.lua
deleted file mode 100644
index 43a4a4c..0000000
--- a/ros/google_cartographer/configuration_files/backpack_3d.lua
+++ /dev/null
@@ -1,32 +0,0 @@
--- Copyright 2016 The Cartographer Authors
---
--- Licensed under the Apache License, Version 2.0 (the "License");
--- you may not use this file except in compliance with the License.
--- You may obtain a copy of the License at
---
--- http://www.apache.org/licenses/LICENSE-2.0
---
--- Unless required by applicable law or agreed to in writing, software
--- distributed under the License is distributed on an "AS IS" BASIS,
--- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
--- See the License for the specific language governing permissions and
--- limitations under the License.
-
-include "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/ros/google_cartographer/configuration_files/turtlebot.lua b/ros/google_cartographer/configuration_files/turtlebot.lua
deleted file mode 100644
index 2f1ff65..0000000
--- a/ros/google_cartographer/configuration_files/turtlebot.lua
+++ /dev/null
@@ -1,26 +0,0 @@
--- Copyright 2016 The Cartographer Authors
---
--- Licensed under the Apache License, Version 2.0 (the "License");
--- you may not use this file except in compliance with the License.
--- You may obtain a copy of the License at
---
--- http://www.apache.org/licenses/LICENSE-2.0
---
--- Unless required by applicable law or agreed to in writing, software
--- distributed under the License is distributed on an "AS IS" BASIS,
--- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
--- See the License for the specific language governing permissions and
--- limitations under the License.
-
-include "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/ros/google_cartographer/launch/3d_mapping.launch b/ros/google_cartographer/launch/3d_mapping.launch
deleted file mode 100644
index 2671e2b..0000000
--- a/ros/google_cartographer/launch/3d_mapping.launch
+++ /dev/null
@@ -1,100 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- 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/ros/google_cartographer/launch/state.launch b/ros/google_cartographer/launch/state.launch
deleted file mode 100644
index c07f760..0000000
--- a/ros/google_cartographer/launch/state.launch
+++ /dev/null
@@ -1,73 +0,0 @@
-
-
-
-
-
-
-
-
-
- # 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/ros/google_cartographer/launch/turtlebot.launch b/ros/google_cartographer/launch/turtlebot.launch
deleted file mode 100644
index 75c9576..0000000
--- a/ros/google_cartographer/launch/turtlebot.launch
+++ /dev/null
@@ -1,41 +0,0 @@
-
-
-
-
-
-
-
-
-
- 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/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program b/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program
deleted file mode 100644
index 045f381..0000000
--- a/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-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/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag
deleted file mode 100644
index 195d813..0000000
--- a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert
deleted file mode 100644
index 74d1bba..0000000
--- a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag b/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag
deleted file mode 100644
index 1998f53..0000000
--- a/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert b/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert
deleted file mode 100644
index 864bfa1..0000000
--- a/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material b/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material
deleted file mode 100644
index cd850fd..0000000
--- a/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-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/ros/google_cartographer/ogre_media/materials/scripts/submap.material b/ros/google_cartographer/ogre_media/materials/scripts/submap.material
deleted file mode 100644
index 1304156..0000000
--- a/ros/google_cartographer/ogre_media/materials/scripts/submap.material
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-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/ros/google_cartographer/package.xml b/ros/google_cartographer/package.xml
deleted file mode 100644
index bf3d0b9..0000000
--- a/ros/google_cartographer/package.xml
+++ /dev/null
@@ -1,68 +0,0 @@
-
-
-
-
- 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/ros/google_cartographer/rviz_plugin_description.xml b/ros/google_cartographer/rviz_plugin_description.xml
deleted file mode 100644
index b990bd7..0000000
--- a/ros/google_cartographer/rviz_plugin_description.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
-
-
- Displays Google Cartographer's submaps as a unified map in RViz.
-
-
-
diff --git a/ros/google_cartographer/src/cartographer_node_main.cc b/ros/google_cartographer/src/cartographer_node_main.cc
deleted file mode 100644
index 5dc619a..0000000
--- a/ros/google_cartographer/src/cartographer_node_main.cc
+++ /dev/null
@@ -1,644 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include
-#include
-#include
-#include
-
-#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/ros/google_cartographer/src/drawable_submap.cc b/ros/google_cartographer/src/drawable_submap.cc
deleted file mode 100644
index 5e2a035..0000000
--- a/ros/google_cartographer/src/drawable_submap.cc
+++ /dev/null
@@ -1,269 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/drawable_submap.h b/ros/google_cartographer/src/drawable_submap.h
deleted file mode 100644
index cbdd53f..0000000
--- a/ros/google_cartographer/src/drawable_submap.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/src/msg_conversion.cc b/ros/google_cartographer/src/msg_conversion.cc
deleted file mode 100644
index 31a18e4..0000000
--- a/ros/google_cartographer/src/msg_conversion.cc
+++ /dev/null
@@ -1,261 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/msg_conversion.h b/ros/google_cartographer/src/msg_conversion.h
deleted file mode 100644
index f553d04..0000000
--- a/ros/google_cartographer/src/msg_conversion.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/src/node_constants.h b/ros/google_cartographer/src/node_constants.h
deleted file mode 100644
index 84c3330..0000000
--- a/ros/google_cartographer/src/node_constants.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/src/submaps_display.cc b/ros/google_cartographer/src/submaps_display.cc
deleted file mode 100644
index 10e24f4..0000000
--- a/ros/google_cartographer/src/submaps_display.cc
+++ /dev/null
@@ -1,331 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/submaps_display.h b/ros/google_cartographer/src/submaps_display.h
deleted file mode 100644
index f3e85e6..0000000
--- a/ros/google_cartographer/src/submaps_display.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/src/time_conversion.cc b/ros/google_cartographer/src/time_conversion.cc
deleted file mode 100644
index 7dfda18..0000000
--- a/ros/google_cartographer/src/time_conversion.cc
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/time_conversion.h b/ros/google_cartographer/src/time_conversion.h
deleted file mode 100644
index 863d871..0000000
--- a/ros/google_cartographer/src/time_conversion.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/src/time_conversion_test.cc b/ros/google_cartographer/src/time_conversion_test.cc
deleted file mode 100644
index 693a67b..0000000
--- a/ros/google_cartographer/src/time_conversion_test.cc
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/trajectory.cc b/ros/google_cartographer/src/trajectory.cc
deleted file mode 100644
index 995a605..0000000
--- a/ros/google_cartographer/src/trajectory.cc
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "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/ros/google_cartographer/src/trajectory.h b/ros/google_cartographer/src/trajectory.h
deleted file mode 100644
index 12ff234..0000000
--- a/ros/google_cartographer/src/trajectory.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#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/ros/google_cartographer/urdf/backpack_2d.urdf b/ros/google_cartographer/urdf/backpack_2d.urdf
deleted file mode 100644
index e72be50..0000000
--- a/ros/google_cartographer/urdf/backpack_2d.urdf
+++ /dev/null
@@ -1,85 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/ros/google_cartographer/urdf/backpack_3d.urdf b/ros/google_cartographer/urdf/backpack_3d.urdf
deleted file mode 100644
index 89f32ef..0000000
--- a/ros/google_cartographer/urdf/backpack_3d.urdf
+++ /dev/null
@@ -1,85 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/ros/google_cartographer/urdf/turtlebot.urdf b/ros/google_cartographer/urdf/turtlebot.urdf
deleted file mode 100644
index f97dcb7..0000000
--- a/ros/google_cartographer/urdf/turtlebot.urdf
+++ /dev/null
@@ -1,53 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/ros/google_cartographer_msgs/CMakeLists.txt b/ros/google_cartographer_msgs/CMakeLists.txt
deleted file mode 100644
index 9551707..0000000
--- a/ros/google_cartographer_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,37 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-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/ros/google_cartographer_msgs/msg/SubmapEntry.msg b/ros/google_cartographer_msgs/msg/SubmapEntry.msg
deleted file mode 100644
index 605de24..0000000
--- a/ros/google_cartographer_msgs/msg/SubmapEntry.msg
+++ /dev/null
@@ -1,16 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-uint32 submap_version
-geometry_msgs/Pose pose
diff --git a/ros/google_cartographer_msgs/msg/SubmapList.msg b/ros/google_cartographer_msgs/msg/SubmapList.msg
deleted file mode 100644
index dbc7b08..0000000
--- a/ros/google_cartographer_msgs/msg/SubmapList.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-TrajectorySubmapList[] trajectory
diff --git a/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg b/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg
deleted file mode 100644
index 678a886..0000000
--- a/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-SubmapEntry[] submap
diff --git a/ros/google_cartographer_msgs/package.xml b/ros/google_cartographer_msgs/package.xml
deleted file mode 100644
index a6d2cad..0000000
--- a/ros/google_cartographer_msgs/package.xml
+++ /dev/null
@@ -1,29 +0,0 @@
-
-
-
- google_cartographer_msgs
- 1.0.0
-
- The google_cartographer_msgs package.
-
- Google
- Apache 2.0
-
- catkin
-
- message_generation
-
diff --git a/ros/google_cartographer_msgs/srv/SubmapQuery.srv b/ros/google_cartographer_msgs/srv/SubmapQuery.srv
deleted file mode 100644
index 4463260..0000000
--- a/ros/google_cartographer_msgs/srv/SubmapQuery.srv
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-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