feat(slam): add cartographer_ros

cartographer_ros/cartographer_rviz/CHANGELOG.rst

@@ -0,0 +1,31 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package cartographer_rviz
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.9003 (2024-04-17)
+---------------------
+
+2.0.9002 (2024-03-15)
+---------------------
+* Fix a warning when building against newer Ogre.
+* Fix warnings when building against recent ROS 2 releases.
+* Contributors: Chris Lalancette
+
+2.0.9001 (2023-04-13)
+---------------------
+
+2.0.9000 (2022-03-31)
+---------------------
+* Update to latest upstream, as well as new port to ROS 2.
+
+0.3.0 (2017-11-23)
+------------------
+* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0
+
+0.2.0 (2017-06-19)
+------------------
+* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0
+
+0.1.0 (2017-05-18)
+------------------
+* First unstable development release

cartographer_ros/cartographer_rviz/CMakeLists.txt

@@ -0,0 +1,118 @@
+# Copyright 2016 The Cartographer Authors
+# Copyright 2022 Wyca Robotics (for the ROS2 conversion)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.5)
+
+project(cartographer_rviz)
+set(CMAKE_POSITION_INDEPENDENT_CODE TRUE)
+
+find_package(ament_cmake REQUIRED)
+
+# Default to C++17
+set(CMAKE_CXX_STANDARD 17)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+set(CMAKE_AUTOMOC ON)
+
+find_package(cartographer REQUIRED)
+include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
+set(BUILD_SHARED_LIBS OFF)
+option(BUILD_GRPC "build features that require Cartographer gRPC support" false)
+google_initialize_cartographer_project()
+
+find_package(absl REQUIRED)
+find_package(cartographer_ros REQUIRED)
+find_package(cartographer_ros_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(urdf REQUIRED)
+
+set(CMAKE_THREAD_PREFER_PTHREAD TRUE)
+set(THREADS_PREFER_PTHREAD_FLAG TRUE)
+find_package(Threads REQUIRED)
+
+set(rviz_plugins_headers_to_moc
+  include/cartographer_rviz/drawable_submap.h
+  include/cartographer_rviz/ogre_slice.h
+  include/cartographer_rviz/submaps_display.h
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/drawable_submap.cpp
+  src/ogre_slice.cpp
+  src/submaps_display.cpp
+  ${rviz_plugins_headers_to_moc}
+)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+)
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  absl::synchronization
+  cartographer
+  cartographer_ros::cartographer_ros
+  Eigen3::Eigen
+  rclcpp::rclcpp
+  rviz_common::rviz_common
+  rviz_rendering::rviz_rendering
+  tf2_ros::tf2_ros
+  tf2_eigen::tf2_eigen
+)
+target_link_libraries(${PROJECT_NAME} PRIVATE
+  pluginlib::pluginlib
+)
+
+pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml)
+register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts")
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include/${PROJECT_NAME})
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  absl
+  cartographer
+  cartographer_ros
+  eigen
+  rclcpp
+  rviz_common
+  rviz_rendering
+  tf2_ros
+)
+
+ament_package()

cartographer_ros/cartographer_rviz/include/cartographer_rviz/drawable_submap.h

@@ -0,0 +1,127 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_
+#define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_
+
+#include <future>
+#include <memory>
+#include <dlfcn.h>
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "OgreSceneManager.h"
+#include "OgreSceneNode.h"
+#include "absl/synchronization/mutex.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer_ros/submap.h"
+#include "cartographer_ros_msgs/msg/submap_entry.hpp"
+#include "cartographer_ros_msgs/srv/submap_query.hpp"
+#include "cartographer_rviz/ogre_slice.h"
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/frame_manager_iface.hpp>
+#include "rviz_rendering/objects/axes.hpp"
+#include "rviz_rendering/objects/movable_text.hpp"
+#include <rviz_common/properties/bool_property.hpp>
+
+namespace cartographer_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:
+  DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
+                 ::rviz_common::DisplayContext* display_context,
+                 Ogre::SceneNode* map_node, ::rviz_common::properties::Property* submap_category,
+                 bool visible, const bool pose_axes_visible,
+                 float pose_axes_length, float pose_axes_radius);
+  ~DrawableSubmap() override;
+  DrawableSubmap(const DrawableSubmap&) = delete;
+  DrawableSubmap& operator=(const DrawableSubmap&) = delete;
+
+  // Updates the 'metadata' for this submap. If necessary, the next call to
+  // MaybeFetchTexture() will fetch a new submap texture.
+  void Update(const ::std_msgs::msg::Header& header,
+              const ::cartographer_ros_msgs::msg::SubmapEntry& metadata);
+
+  // If an update is needed, it will send an RPC using 'client' to request the
+  // new data for the submap and returns true.
+  bool MaybeFetchTexture(rclcpp::Client<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr client,
+                         rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor);
+
+  // Returns whether an RPC is in progress.
+  bool QueryInProgress();
+
+  // Sets the alpha of the submap taking into account its slice height and the
+  // 'current_tracking_z'. 'fade_out_start_distance_in_meters' defines the
+  // distance in z direction in meters, before which the submap will be shown
+  // at full opacity.
+  void SetAlpha(double current_tracking_z, float fade_out_distance_in_meters);
+
+  // Sets the visibility of a slice. It will be drawn if the parent submap
+  // is also visible.
+  void SetSliceVisibility(size_t slice_index, bool visible);
+
+  ::cartographer::mapping::SubmapId id() const { return id_; }
+  int version() const { return metadata_version_; }
+  bool visibility() const { return visibility_->getBool(); }
+  void set_visibility(const bool visibility) {
+    visibility_->setBool(visibility);
+  }
+  void set_pose_markers_visibility(const bool visibility) {
+    pose_axes_visible_ = visibility;
+    TogglePoseMarkerVisibility();
+  }
+
+ Q_SIGNALS:
+  // RPC request succeeded.
+  void RequestSucceeded();
+
+ private Q_SLOTS:
+  // Callback when an rpc request succeeded.
+  void UpdateSceneNode();
+  void ToggleVisibility();
+  void TogglePoseMarkerVisibility();
+
+ private:
+  const ::cartographer::mapping::SubmapId id_;
+
+  absl::Mutex mutex_;
+  ::rviz_common::DisplayContext* const display_context_;
+  Ogre::SceneNode* const submap_node_;
+  Ogre::SceneNode* const submap_id_text_node_;
+  std::vector<std::unique_ptr<OgreSlice>> ogre_slices_;
+  ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
+  ::rviz_rendering::Axes pose_axes_;
+  bool pose_axes_visible_;
+  ::rviz_rendering::MovableText submap_id_text_;
+  std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
+  bool query_in_progress_ GUARDED_BY(mutex_) = false;
+  int metadata_version_ GUARDED_BY(mutex_) = -1;
+  std::future<void> rpc_request_future_;
+  std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_
+      GUARDED_BY(mutex_);
+  float current_alpha_ = 0.f;
+  std::unique_ptr<::rviz_common::properties::BoolProperty> visibility_;
+};
+
+}  // namespace cartographer_rviz
+
+#endif  // CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_

cartographer_ros/cartographer_rviz/include/cartographer_rviz/ogre_slice.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_
+#define CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "OgreManualObject.h"
+#include "OgreMaterial.h"
+#include "OgreQuaternion.h"
+#include "OgreSceneManager.h"
+#include "OgreSceneNode.h"
+#include "OgreTexture.h"
+#include "Ogre.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/id.h"
+
+namespace cartographer_rviz {
+
+Ogre::Vector3 ToOgre(const Eigen::Vector3d& v);
+Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q);
+
+// A class containing the Ogre code to visualize a slice texture of a submap.
+// Member functions are expected to be called from the Ogre thread.
+class OgreSlice {
+ public:
+  // Attaches a node visualizing the submap 'id' to the 'submap_node' which is
+  // expected to represent the submap frame.
+  OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
+            Ogre::SceneManager* const scene_manager,
+            Ogre::SceneNode* const submap_node);
+  ~OgreSlice();
+
+  OgreSlice(const OgreSlice&) = delete;
+  OgreSlice& operator=(const OgreSlice&) = delete;
+
+  // Updates the texture and pose of the submap using new data from
+  // 'submap_texture'.
+  void Update(const ::cartographer::io::SubmapTexture& submap_texture);
+
+  // Changes the opacity of the submap to 'alpha'.
+  void SetAlpha(float alpha);
+
+  // Sets the local visibility of this slice.
+  void SetVisibility(bool visibility);
+
+  // Updates the SceneNode to be visible if the submap and this slice are
+  // visible.
+  void UpdateOgreNodeVisibility(bool submap_visibility);
+
+ private:
+  // TODO(gaschler): Pack both ids into a struct.
+  const ::cartographer::mapping::SubmapId id_;
+  const int slice_id_;
+  Ogre::SceneManager* const scene_manager_;
+  Ogre::SceneNode* const submap_node_;
+  Ogre::SceneNode* const slice_node_;
+  Ogre::ManualObject* const manual_object_;
+  Ogre::TexturePtr texture_;
+  Ogre::MaterialPtr material_;
+  bool visibility_ = true;
+};
+
+}  // namespace cartographer_rviz
+
+#endif  // CARTOGRAPHER_RVIZ_SRC_OGRE_SLICE_H_

cartographer_ros/cartographer_rviz/include/cartographer_rviz/submaps_display.h

@@ -0,0 +1,113 @@
+/*
+ * 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_RVIZ_SRC_SUBMAPS_DISPLAY_H_
+#define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <dlfcn.h>
+#include "absl/synchronization/mutex.h"
+#include "cartographer/common/port.h"
+#include "cartographer_ros_msgs/msg/submap_list.hpp"
+#include "cartographer_rviz/drawable_submap.h"
+#include <rviz_common/message_filter_display.hpp>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace cartographer_rviz {
+
+// TODO(gaschler): This should be a private class in SubmapsDisplay,
+// unfortunately, QT does not allow for this. Move the logic out of the struct
+// and use just one slot for all changes.
+struct Trajectory : public QObject {
+  Q_OBJECT
+
+ public:
+  Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property,
+             bool pose_markers_enabled);
+
+  std::unique_ptr<::rviz_common::properties::BoolProperty> visibility;
+  std::unique_ptr<::rviz_common::properties::BoolProperty> pose_markers_visibility;
+  std::map<int, std::unique_ptr<DrawableSubmap>> submaps;
+
+ private Q_SLOTS:
+  void AllEnabledToggled();
+  void PoseMarkersEnabledToggled();
+};
+
+// RViz plugin used for displaying maps which are represented by a collection of
+// submaps.
+//
+// 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.
+class SubmapsDisplay
+    : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node {
+  Q_OBJECT
+
+ public:
+  SubmapsDisplay();
+  ~SubmapsDisplay() override;
+
+
+
+  SubmapsDisplay(const SubmapsDisplay&) = delete;
+  SubmapsDisplay& operator=(const SubmapsDisplay&) = delete;
+
+ private Q_SLOTS:
+  void Reset();
+  void AllEnabledToggled();
+  void PoseMarkersEnabledToggled();
+  void ResolutionToggled();
+
+ private:
+  void CreateClient();
+
+  // These are called by RViz and therefore do not adhere to the style guide.
+  void onInitialize() override;
+  void reset() override;
+  void processMessage(
+        const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override;
+  void update(float , float) override;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_;
+  rclcpp::Client<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr client_;
+  ::rviz_common::properties::StringProperty* submap_query_service_property_;
+  std::unique_ptr<std::string> map_frame_;
+  ::rviz_common::properties::StringProperty* tracking_frame_property_;
+  Ogre::SceneNode* map_node_ = nullptr;  // Represents the map frame.
+  std::map<int, std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
+  absl::Mutex mutex_;
+  ::rviz_common::properties::BoolProperty* slice_high_resolution_enabled_;
+  ::rviz_common::properties::BoolProperty* slice_low_resolution_enabled_;
+  ::rviz_common::properties::Property* trajectories_category_;
+  ::rviz_common::properties::BoolProperty* visibility_all_enabled_;
+  ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_;
+  ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_;
+
+};
+
+}  // namespace cartographer_rviz
+
+#endif  // CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_

cartographer_ros/cartographer_rviz/ogre_media/materials/glsl120/glsl120.program

@@ -0,0 +1,25 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+fragment_program cartographer_ros/glsl120/submap.frag glsl
+{
+  source submap.frag
+}
+
+vertex_program cartographer_ros/glsl120/submap.vert glsl
+{
+  source submap.vert
+}

cartographer_ros/cartographer_rviz/ogre_media/materials/glsl120/submap.frag

@@ -0,0 +1,30 @@
+/*
+ * 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 = u_alpha * texture_value.r;
+  float alpha = u_alpha * texture_value.g;
+  gl_FragColor = vec4(value, value, value, alpha);
+}

cartographer_ros/cartographer_rviz/ogre_media/materials/glsl120/submap.vert

@@ -0,0 +1,27 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#version 120
+
+attribute vec4 uv0;
+
+varying vec2 out_submap_texture_coordinate;
+
+void main()
+{
+  out_submap_texture_coordinate = vec2(uv0);
+  gl_Position = ftransform();
+}

cartographer_ros/cartographer_rviz/ogre_media/materials/scripts/submap.material

@@ -0,0 +1,32 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+material cartographer_ros/Submap
+{
+  technique
+  {
+    pass
+    {
+      vertex_program_ref cartographer_ros/glsl120/submap.vert {}
+
+      fragment_program_ref cartographer_ros/glsl120/submap.frag
+      {
+        param_named u_submap int 0
+        param_named u_alpha float 1.0
+      }
+    }
+  }
+}

cartographer_ros/cartographer_rviz/package.xml

@@ -0,0 +1,89 @@
+<?xml version="1.0"?>
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<package format="3">
+  <name>cartographer_rviz</name>
+  <!--
+  As of 2022-03-31, the versioning for cartographer_ros is in a complicated situation.
+  Upstream https://github.com/cartographer-project/cartographer_ros seems to be moribund,
+  with no commits for close to a year.  The master branch there is only for ROS 1, and
+  that branch has not had a released tag since 1.0.0 in 2018.
+
+  In the meantime, we've done a port to ROS 2 in https://github.com/ros2/cartographer_ros
+  that targets upstream cartographer 2.0.0.
+
+  Since we have no response from upstream in a long time, we are going to go ahead and
+  assume that the 1.x series of tags from upstream cartographer_ros will always be used
+  to target ROS 1.  Further we are going to assume that the 2.x series of tags
+  from upstream cartographer_ros (none of which exist yet) will be used to target ROS 2.
+  If upstream comes alive and chooses a different versioning scheme, we can adjust later
+  as necessary.  The outcome of all of this is that we are assuming a starting "upstream"
+  release of 2.0.0, which then plays into the rest of the explanation below.
+
+  We add 900 to the patch part of the version and then multiply it by 10,
+  i.e. our version = `(upstream_patch_version + 900) * 10`,
+  so we can have intermediate releases as well as release any future official 2.0.x versions.
+
+  This is basically packing the patch part of the version and a fourth version part together
+  into the third part of the version.
+
+  The use of `900` instead of something else like `100` is arbitrary, but it might
+  help people recognize that this is a "special" version number.
+  It is needed however, because we cannot have a leading `0` in our patch version.
+
+  Consider these possible future versions as an example:
+
+  2.0.9000 -> current state of this repository, 2.0.0 + some commits from us
+  2.0.9010 -> upstream 2.0.1
+  2.0.9011 -> upstream 2.0.1 + additional commits from upstream or us
+  2.0.9012 -> upstream 2.0.1 + additional commits from 2.0.1011 + more new commits
+  2.0.9020 -> upstream 2.0.2
+  and so on...
+  -->
+  <version>2.0.9003</version>
+  <description>
+    Cartographer is a system that provides real-time simultaneous localization
+    and mapping (SLAM) in 2D and 3D across multiple platforms and sensor
+    configurations. This package provides Cartographer's RViz integration.
+  </description>
+  <maintainer email="clalancette@openrobotics.org">Chris Lalancette</maintainer>
+  <license>Apache 2.0</license>
+
+  <url>https://github.com/cartographer-project/cartographer_ros</url>
+
+  <author email="google-cartographer@googlegroups.com">
+    The Cartographer Authors
+  </author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>cartographer</depend>
+  <depend>cartographer_ros</depend>
+  <depend>cartographer_ros_msgs</depend>
+  <depend>eigen</depend>
+  <depend>libabsl-dev</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>rviz_rendering</depend>
+
+  <export>
+      <build_type>ament_cmake</build_type>
+      <rviz plugin="rviz_plugin_description.xml" />
+  </export>
+</package>

cartographer_ros/cartographer_rviz/rviz_plugin_description.xml

@@ -0,0 +1,26 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<library path="cartographer_rviz">
+  <class name="cartographer_rviz/SubmapsDisplay"
+         type="cartographer_rviz::SubmapsDisplay"
+         base_class_type="rviz_common::Display">
+    <description>
+      Displays submaps as a unified map in RViz.
+      https://github.com/cartographer-project/cartographer_ros
+    </description>
+  </class>
+</library>

cartographer_ros/cartographer_rviz/src/.clang-format

@@ -0,0 +1,2 @@
+BasedOnStyle: Google
+DerivePointerAlignment: false

cartographer_ros/cartographer_rviz/src/drawable_submap.cpp

@@ -0,0 +1,207 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_rviz/drawable_submap.h"
+
+#include <chrono>
+#include <future>
+#include <sstream>
+#include <string>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "absl/memory/memory.h"
+#include "cartographer/common/port.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros_msgs/srv/submap_query.hpp"
+#include <rclcpp/rclcpp.hpp>
+
+
+namespace cartographer_rviz {
+
+namespace {
+
+constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
+constexpr float kAlphaUpdateThreshold = 0.2f;
+
+const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red);
+const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3);
+constexpr float kSubmapIdCharHeight = 0.2f;
+constexpr int kNumberOfSlicesPerSubmap = 2;
+
+}  // namespace
+
+
+DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
+                               ::rviz_common::DisplayContext* const display_context,
+                               Ogre::SceneNode* const map_node,
+                               ::rviz_common::properties::Property* const submap_category,
+                               const bool visible, const bool pose_axes_visible,
+                               const float pose_axes_length,
+                               const float pose_axes_radius)
+    : id_(id),
+      display_context_(display_context),
+      submap_node_(map_node->createChildSceneNode()),
+      submap_id_text_node_(submap_node_->createChildSceneNode()),
+      pose_axes_(display_context->getSceneManager(), submap_node_,
+                 pose_axes_length, pose_axes_radius),
+      pose_axes_visible_(pose_axes_visible),
+      submap_id_text_(QString("(%1,%2)")
+                          .arg(id.trajectory_id)
+                          .arg(id.submap_index)
+                          .toStdString()),
+      last_query_timestamp_(0){
+  for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap;
+       ++slice_index) {
+    ogre_slices_.emplace_back(absl::make_unique<OgreSlice>(
+        id, slice_index, display_context->getSceneManager(), submap_node_));
+  }
+  // DrawableSubmap creates and manages its visibility property object
+  // (a unique_ptr is needed because the Qt parent of the visibility
+  // property is the submap_category object - the BoolProperty needs
+  // to be destroyed along with the DrawableSubmap)
+  visibility_ = absl::make_unique<::rviz_common::properties::BoolProperty>(
+      "" /* title */, visible, "" /* description */, submap_category,
+      SLOT(ToggleVisibility()), this);
+  submap_id_text_.setCharacterHeight(kSubmapIdCharHeight);
+  submap_id_text_.setColor(kSubmapIdColor);
+  submap_id_text_.setTextAlignment(::rviz_rendering::MovableText::H_CENTER,
+                                   ::rviz_rendering::MovableText::V_ABOVE);
+  submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition));
+  submap_id_text_node_->attachObject(&submap_id_text_);
+  TogglePoseMarkerVisibility();
+  connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
+}
+
+DrawableSubmap::~DrawableSubmap() {
+  // 'query_in_progress_' must be true until the Q_EMIT has happened. Qt then
+  // makes sure that 'RequestSucceeded' is not called after our destruction.
+  if (QueryInProgress()) {
+    rpc_request_future_.wait();
+  }
+  display_context_->getSceneManager()->destroySceneNode(submap_node_);
+  display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_);
+}
+
+void DrawableSubmap::Update(
+    const ::std_msgs::msg::Header& header,
+    const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) {
+  (void) header; // TODO: remove unused arg ?
+  absl::MutexLock locker(&mutex_);
+  metadata_version_ = metadata.submap_version;
+  pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
+  submap_node_->setPosition(ToOgre(pose_.translation()));
+  submap_node_->setOrientation(ToOgre(pose_.rotation()));
+  display_context_->queueRender();
+  visibility_->setName(
+      QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
+  visibility_->setDescription(
+      QString("Toggle visibility of this individual submap.<br><br>"
+              "Trajectory %1, submap %2, submap version %3")
+          .arg(id_.trajectory_id)
+          .arg(id_.submap_index)
+          .arg(metadata_version_));
+}
+
+bool DrawableSubmap::MaybeFetchTexture(
+    rclcpp::Client<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr const client,
+    rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor) {
+  absl::MutexLock locker(&mutex_);
+  // Received metadata version can also be lower if we restarted Cartographer.
+  const bool newer_version_available =
+      submap_textures_ == nullptr ||
+      submap_textures_->version != metadata_version_;
+  const std::chrono::milliseconds now =
+      std::chrono::duration_cast<std::chrono::milliseconds>(
+          std::chrono::system_clock::now().time_since_epoch());
+  const bool recently_queried =
+      last_query_timestamp_ + kMinQueryDelayInMs > now;
+  if (!newer_version_available || recently_queried || query_in_progress_) {
+    return false;
+  }
+  query_in_progress_ = true;
+  last_query_timestamp_ = now;
+
+  rpc_request_future_ = std::async(std::launch::async, [this, client, callback_group_executor]() {
+    std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures =
+        ::cartographer_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000));
+    absl::MutexLock locker(&mutex_);
+    query_in_progress_ = false;
+    if (submap_textures != nullptr) {
+      // We emit a signal to update in the right thread, and pass via the
+      // 'submap_texture_' member to simplify the signal-slot connection
+      // slightly.
+      submap_textures_ = std::move(submap_textures);
+      Q_EMIT RequestSucceeded();
+    }
+  });
+  return true;
+}
+
+bool DrawableSubmap::QueryInProgress() {
+  absl::MutexLock locker(&mutex_);
+  return query_in_progress_;
+}
+
+void DrawableSubmap::SetAlpha(const double current_tracking_z,
+                              const float fade_out_start_distance_in_meters) {
+  const float fade_out_distance_in_meters =
+      2.f * fade_out_start_distance_in_meters;
+  const double distance_z =
+      std::abs(pose_.translation().z() - current_tracking_z);
+  const double fade_distance =
+      std::max(distance_z - fade_out_start_distance_in_meters, 0.);
+  const float target_alpha = static_cast<float>(
+      std::max(0., 1. - fade_distance / fade_out_distance_in_meters));
+
+  if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
+      target_alpha == 0.f || target_alpha == 1.f) {
+    current_alpha_ = target_alpha;
+  }
+  for (auto& slice : ogre_slices_) {
+    slice->SetAlpha(current_alpha_);
+  }
+  display_context_->queueRender();
+}
+
+void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) {
+  ogre_slices_.at(slice_index)->SetVisibility(visible);
+  ToggleVisibility();
+}
+
+void DrawableSubmap::UpdateSceneNode() {
+  absl::MutexLock locker(&mutex_);
+  for (size_t slice_index = 0; slice_index < ogre_slices_.size() &&
+                               slice_index < submap_textures_->textures.size();
+       ++slice_index) {
+    ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]);
+  }
+  display_context_->queueRender();
+}
+
+void DrawableSubmap::ToggleVisibility() {
+  for (auto& ogre_slice : ogre_slices_) {
+    ogre_slice->UpdateOgreNodeVisibility(visibility_->getBool());
+  }
+  display_context_->queueRender();
+}
+
+void DrawableSubmap::TogglePoseMarkerVisibility() {
+  submap_id_text_node_->setVisible(pose_axes_visible_);
+  pose_axes_.getSceneNode()->setVisible(pose_axes_visible_);
+}
+
+}  // namespace cartographer_rviz

cartographer_ros/cartographer_rviz/src/ogre_slice.cpp

@@ -0,0 +1,154 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_rviz/ogre_slice.h"
+
+#include <string>
+#include <vector>
+
+#include "OgreGpuProgramParams.h"
+#include "OgreImage.h"
+#include "OgreMaterialManager.h"
+#include "OgreTechnique.h"
+#include "OgreTextureManager.h"
+#include "cartographer/common/port.h"
+
+namespace cartographer_rviz {
+
+namespace {
+
+constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
+constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
+constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
+constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
+
+std::string GetSliceIdentifier(
+    const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) {
+    return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id));
+}
+
+}  // namespace
+
+Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) {
+  return Ogre::Vector3(v.x(), v.y(), v.z());
+}
+
+Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
+  return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z());
+}
+
+OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
+                     Ogre::SceneManager* const scene_manager,
+                     Ogre::SceneNode* const submap_node)
+    : id_(id),
+      slice_id_(slice_id),
+      scene_manager_(scene_manager),
+      submap_node_(submap_node),
+      slice_node_(submap_node_->createChildSceneNode()),
+      manual_object_(scene_manager_->createManualObject(
+          kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) {
+  material_ = Ogre::MaterialManager::getSingleton().getByName(kSubmapSourceMaterialName);
+  material_ = material_->clone(
+      kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_), true, "General");
+  material_->setReceiveShadows(false);
+  material_->getTechnique(0)->setLightingEnabled(false);
+  material_->setCullingMode(Ogre::CULL_NONE);
+  material_->setDepthBias(-1.f, 0.f);
+  material_->setDepthWriteEnabled(false);
+  slice_node_->attachObject(manual_object_);
+}
+
+OgreSlice::~OgreSlice() {
+  Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
+  if (texture_) {
+    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
+    texture_.reset();
+  }
+  scene_manager_->destroySceneNode(slice_node_);
+  scene_manager_->destroyManualObject(manual_object_);
+}
+
+void OgreSlice::Update(
+    const ::cartographer::io::SubmapTexture& submap_texture) {
+  slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
+  slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
+  // The call to Ogre's loadRawData below does not work with an RG texture,
+  // therefore we create an RGB one whose blue channel is always 0.
+  std::vector<char> rgb;
+  CHECK_EQ(submap_texture.pixels.intensity.size(),
+           submap_texture.pixels.alpha.size());
+  for (size_t i = 0; i < submap_texture.pixels.intensity.size(); ++i) {
+    rgb.push_back(submap_texture.pixels.intensity[i]);
+    rgb.push_back(submap_texture.pixels.alpha[i]);
+    rgb.push_back(0);
+  }
+
+  manual_object_->clear();
+  const float metric_width = submap_texture.resolution * submap_texture.width;
+  const float metric_height = submap_texture.resolution * submap_texture.height;
+  manual_object_->begin(material_->getName(),
+                        Ogre::RenderOperation::OT_TRIANGLE_STRIP);
+  // Bottom left
+  manual_object_->position(-metric_height, 0.0f, 0.0f);
+  manual_object_->textureCoord(0.0f, 1.0f);
+  // Bottom right
+  manual_object_->position(-metric_height, -metric_width, 0.0f);
+  manual_object_->textureCoord(1.0f, 1.0f);
+  // Top left
+  manual_object_->position(0.0f, 0.0f, 0.0f);
+  manual_object_->textureCoord(0.0f, 0.0f);
+  // Top right
+  manual_object_->position(0.0f, -metric_width, 0.0f);
+  manual_object_->textureCoord(1.0f, 0.0f);
+  manual_object_->end();
+
+  Ogre::DataStreamPtr pixel_stream;
+  pixel_stream.reset(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
+
+  if (texture_) {
+    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
+    texture_.reset();
+  }
+  const std::string texture_name =
+      kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_);
+  texture_ = Ogre::TextureManager::getSingleton().loadRawData(
+      texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+      pixel_stream, submap_texture.width, submap_texture.height,
+      Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
+
+  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 OgreSlice::SetAlpha(const float alpha) {
+  const Ogre::GpuProgramParametersSharedPtr parameters =
+      material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
+  parameters->setNamedConstant("u_alpha", alpha);
+}
+
+void OgreSlice::SetVisibility(bool visibility) { visibility_ = visibility; }
+
+void OgreSlice::UpdateOgreNodeVisibility(bool submap_visibility) {
+  slice_node_->setVisible(submap_visibility && visibility_);
+}
+
+}  // namespace cartographer_rviz

cartographer_ros/cartographer_rviz/src/submaps_display.cpp

@@ -0,0 +1,339 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_rviz/submaps_display.h"
+
+#include "OgreResourceGroupManager.h"
+#include "absl/memory/memory.h"
+#include "absl/synchronization/mutex.h"
+
+#include "cartographer/mapping/id.h"
+#include "cartographer_ros_msgs/msg/submap_list.hpp"
+#include "cartographer_ros_msgs/srv/submap_query.hpp"
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <pluginlib/class_list_macros.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/version.h>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/frame_manager_iface.hpp>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/string_property.hpp>
+#include <rviz_common/message_filter_display.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace cartographer_rviz {
+
+namespace {
+
+constexpr int kMaxOnGoingRequestsPerTrajectory = 6;
+constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
+constexpr char kGlsl120Directory[] = "/glsl120";
+constexpr char kScriptsDirectory[] = "/scripts";
+constexpr char kDefaultTrackingFrame[] = "base_link";
+constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query";
+
+}  // namespace
+
+SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") {
+  submap_query_service_property_ = new ::rviz_common::properties::StringProperty(
+      "Submap query service", kDefaultSubmapQueryServiceName,
+      "Submap query service to connect to.", this, SLOT(Reset()));
+  tracking_frame_property_ = new ::rviz_common::properties::StringProperty(
+      "Tracking frame", kDefaultTrackingFrame,
+      "Tracking frame, used for fading out submaps.", this);
+  slice_high_resolution_enabled_ = new ::rviz_common::properties::BoolProperty(
+      "High Resolution", true, "Display high resolution slices.", this,
+      SLOT(ResolutionToggled()), this);
+  slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty(
+      "Low Resolution", false, "Display low resolution slices.", this,
+      SLOT(ResolutionToggled()), this);
+
+  callback_group_ = this->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface());
+  client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(
+        kDefaultSubmapQueryServiceName,
+#if RCLCPP_VERSION_GTE(17, 0, 0)
+        rclcpp::ServicesQoS(),
+#else
+        rmw_qos_profile_services_default,
+#endif
+        callback_group_
+        );
+  trajectories_category_ = new ::rviz_common::properties::Property(
+      "Submaps", QVariant(), "List of all submaps, organized by trajectories.",
+      this);
+  visibility_all_enabled_ = new ::rviz_common::properties::BoolProperty(
+      "All", true,
+      "Whether submaps from all trajectories should be displayed or not.",
+      trajectories_category_, SLOT(AllEnabledToggled()), this);
+  pose_markers_all_enabled_ = new ::rviz_common::properties::BoolProperty(
+      "All Submap Pose Markers", true,
+      "Whether submap pose markers should be displayed or not.",
+      trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this);
+  fade_out_start_distance_in_meters_ =
+      new ::rviz_common::properties::FloatProperty("Fade-out distance", 1.f,
+                                "Distance in meters in z-direction beyond "
+                                "which submaps will start to fade out.",
+                                this);
+  const std::string package_path = ament_index_cpp::get_package_share_directory("cartographer_rviz");
+  Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+      package_path + kMaterialsDirectory, "FileSystem", "cartographer_rviz");
+  Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+      package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem",
+      "cartographer_rviz");
+  Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+      package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem",
+      "cartographer_rviz");
+  Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
+
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+SubmapsDisplay::~SubmapsDisplay() {
+  client_.reset();
+  trajectories_.clear();
+  scene_manager_->destroySceneNode(map_node_);
+}
+
+void SubmapsDisplay::Reset() { reset(); }
+
+void SubmapsDisplay::CreateClient() {
+  client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(
+      submap_query_service_property_->getStdString(),
+#if RCLCPP_VERSION_GTE(17, 0, 0)
+      rclcpp::ServicesQoS(),
+#else
+      rmw_qos_profile_services_default,
+#endif
+      callback_group_
+      );
+}
+
+void SubmapsDisplay::onInitialize() {
+  MFDClass::onInitialize();
+  map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+  CreateClient();
+}
+
+void SubmapsDisplay::reset() {
+  MFDClass::reset();
+  absl::MutexLock locker(&mutex_);
+  client_.reset();
+  trajectories_.clear();
+  CreateClient();
+}
+
+void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) {
+  absl::MutexLock locker(&mutex_);
+  map_frame_ = absl::make_unique<std::string>(msg->header.frame_id);
+  // In case Cartographer node is relaunched, destroy trajectories from the
+  // previous instance.
+  for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) {
+    const size_t trajectory_id = submap_entry.trajectory_id;
+    if (trajectories_.count(trajectory_id) == 0) {
+      continue;
+    }
+    const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
+    const auto it = trajectory_submaps.find(submap_entry.submap_index);
+    if (it != trajectory_submaps.end() &&
+        it->second->version() > submap_entry.submap_version) {
+      // Versions should only increase unless Cartographer restarted.
+      trajectories_.clear();
+      break;
+    }
+  }
+  using ::cartographer::mapping::SubmapId;
+  std::set<SubmapId> listed_submaps;
+  std::set<int> listed_trajectories;
+  for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) {
+    const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
+    listed_submaps.insert(id);
+    listed_trajectories.insert(submap_entry.trajectory_id);
+    if (trajectories_.count(id.trajectory_id) == 0) {
+      trajectories_.insert(std::make_pair(
+          id.trajectory_id,
+          absl::make_unique<Trajectory>(
+              absl::make_unique<::rviz_common::properties::BoolProperty>(
+                  QString("Trajectory %1").arg(id.trajectory_id),
+                  visibility_all_enabled_->getBool(),
+                  QString(
+                      "List of all submaps in Trajectory %1. The checkbox "
+                      "controls whether all submaps in this trajectory should "
+                      "be displayed or not.")
+                      .arg(id.trajectory_id),
+                  trajectories_category_),
+              pose_markers_all_enabled_->getBool())));
+    }
+    auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
+    auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
+    auto& pose_markers_visibility =
+        trajectories_[id.trajectory_id]->pose_markers_visibility;
+    if (trajectory_submaps.count(id.submap_index) == 0) {
+      // TODO(ojura): Add RViz properties for adjusting submap pose axes
+      constexpr float kSubmapPoseAxesLength = 0.3f;
+      constexpr float kSubmapPoseAxesRadius = 0.06f;
+      trajectory_submaps.emplace(
+          id.submap_index,
+          absl::make_unique<DrawableSubmap>(
+              id, context_, map_node_, trajectory_visibility.get(),
+              trajectory_visibility->getBool(),
+              pose_markers_visibility->getBool(), kSubmapPoseAxesLength,
+              kSubmapPoseAxesRadius));
+      trajectory_submaps.at(id.submap_index)
+          ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool());
+      trajectory_submaps.at(id.submap_index)
+          ->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool());
+    }
+    trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
+  }
+  // Remove all deleted trajectories not mentioned in the SubmapList.
+  for (auto it = trajectories_.begin(); it != trajectories_.end();) {
+    if (listed_trajectories.count(it->first) == 0) {
+      it = trajectories_.erase(it);
+    } else {
+      ++it;
+    }
+  }
+  // Remove all submaps not mentioned in the SubmapList.
+  for (const auto& trajectory_by_id : trajectories_) {
+    const int trajectory_id = trajectory_by_id.first;
+    auto& trajectory_submaps = trajectory_by_id.second->submaps;
+    for (auto it = trajectory_submaps.begin();
+         it != trajectory_submaps.end();) {
+      if (listed_submaps.count(
+              SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
+        it = trajectory_submaps.erase(it);
+      } else {
+        ++it;
+      }
+    }
+  }
+}
+
+void SubmapsDisplay::update(const float , const float) {
+  absl::MutexLock locker(&mutex_);
+  // Schedule fetching of new submap textures.
+  for (const auto& trajectory_by_id : trajectories_) {
+    int num_ongoing_requests = 0;
+    for (const auto& submap_entry : trajectory_by_id.second->submaps) {
+      if (submap_entry.second->QueryInProgress()) {
+        ++num_ongoing_requests;
+      }
+    }
+    for (auto it = trajectory_by_id.second->submaps.rbegin();
+         it != trajectory_by_id.second->submaps.rend() &&
+         num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
+         ++it) {
+      if (it->second->MaybeFetchTexture(client_, callback_group_executor_)) {
+        ++num_ongoing_requests;
+      }
+    }
+  }
+  if (map_frame_ == nullptr) {
+    return;
+  }
+
+  // Update the fading by z distance.
+  const auto klatest = this->get_clock()->now();
+  try {
+    const ::geometry_msgs::msg::TransformStamped transform_stamped =
+        tf_buffer_->lookupTransform(
+            *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero,std::chrono::milliseconds(100));
+    for (auto& trajectory_by_id : trajectories_) {
+      for (auto& submap_entry : trajectory_by_id.second->submaps) {
+        submap_entry.second->SetAlpha(
+            transform_stamped.transform.translation.z,
+            fade_out_start_distance_in_meters_->getFloat());
+      }
+    }
+  } catch (const tf2::TransformException& ex) {
+    RCLCPP_WARN(this->get_logger(), "Could not compute submap fading: %s", ex.what());
+  }
+  // Update the map frame to fixed frame transform.
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (context_->getFrameManager()->getTransform(*map_frame_, klatest, position,
+                                                orientation)) {
+    map_node_->setPosition(position);
+    map_node_->setOrientation(orientation);
+    context_->queueRender();
+  }
+}
+
+void SubmapsDisplay::AllEnabledToggled() {
+  absl::MutexLock locker(&mutex_);
+  const bool visible = visibility_all_enabled_->getBool();
+  for (auto& trajectory_by_id : trajectories_) {
+    trajectory_by_id.second->visibility->setBool(visible);
+  }
+}
+
+void SubmapsDisplay::PoseMarkersEnabledToggled() {
+  absl::MutexLock locker(&mutex_);
+  const bool visible = pose_markers_all_enabled_->getBool();
+  for (auto& trajectory_by_id : trajectories_) {
+    trajectory_by_id.second->pose_markers_visibility->setBool(visible);
+  }
+}
+
+void SubmapsDisplay::ResolutionToggled() {
+  absl::MutexLock locker(&mutex_);
+  for (auto& trajectory_by_id : trajectories_) {
+    for (auto& submap_entry : trajectory_by_id.second->submaps) {
+      submap_entry.second->SetSliceVisibility(
+          0, slice_high_resolution_enabled_->getBool());
+      submap_entry.second->SetSliceVisibility(
+          1, slice_low_resolution_enabled_->getBool());
+    }
+  }
+}
+
+void Trajectory::AllEnabledToggled() {
+  const bool visible = visibility->getBool();
+  for (auto& submap_entry : submaps) {
+    submap_entry.second->set_visibility(visible);
+  }
+}
+
+void Trajectory::PoseMarkersEnabledToggled() {
+  const bool visible = pose_markers_visibility->getBool();
+  for (auto& submap_entry : submaps) {
+    submap_entry.second->set_pose_markers_visibility(visible);
+  }
+}
+
+Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property,
+                       const bool pose_markers_enabled)
+    : visibility(std::move(property)) {
+  ::QObject::connect(visibility.get(), SIGNAL(changed()), this,
+                     SLOT(AllEnabledToggled()));
+  // Add toggle for submap pose markers as the first entry of the visibility
+  // property list of this trajectory.
+  pose_markers_visibility = absl::make_unique<::rviz_common::properties::BoolProperty>(
+      QString("Submap Pose Markers"), pose_markers_enabled,
+      QString("Toggles the submap pose markers of this trajectory."),
+      visibility.get());
+  ::QObject::connect(pose_markers_visibility.get(), SIGNAL(changed()), this,
+                     SLOT(PoseMarkersEnabledToggled()));
+}
+
+}  // namespace cartographer_rviz
+
+PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, rviz_common::Display)