feat(slam): add rtabmap_ros

rtabmap_ros/rtabmap_rviz_plugins/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_rviz_plugins)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+  # issues #1285 #1288
+  find_library(
+    message_filters_LIB NAMES message_filters
+    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(rtabmap_conversions REQUIRED)
+find_package(rtabmap_msgs REQUIRED)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+SET(Libraries
+  pcl_conversions
+  pluginlib
+  rclcpp
+  rviz_common
+  rviz_rendering
+  rviz_default_plugins
+  sensor_msgs
+  std_msgs
+  tf2
+  rtabmap_conversions
+  rtabmap_msgs
+)
+
+MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}")
+
+
+###########
+## Build ##
+###########
+
+## We also use Ogre for rviz plugins
+include_directories( ${OGRE_INCLUDE_DIRS} )
+
+# tf:message_filters, mixing boost and Qt signals
+set_property(
+    SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp src/OrbitOrientedViewController.cpp
+    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
+)
+
+add_library(rtabmap_rviz_plugins SHARED
+       src/MapCloudDisplay.cpp
+       src/MapGraphDisplay.cpp
+       src/InfoDisplay.cpp
+       include/${PROJECT_NAME}/MapCloudDisplay.h
+       include/${PROJECT_NAME}/MapGraphDisplay.h
+       include/${PROJECT_NAME}/InfoDisplay.h
+)
+set_property(TARGET rtabmap_rviz_plugins PROPERTY AUTOMOC ON)
+target_include_directories(rtabmap_rviz_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(rtabmap_rviz_plugins ${Libraries})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
+
+# prevent pluginlib from using boost
+target_compile_definitions(rtabmap_rviz_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml)
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+install(TARGETS 
+   rtabmap_rviz_plugins
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+   INCLUDES DESTINATION include
+)
+
+ament_package()

rtabmap_ros/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/InfoDisplay.h

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef INFO_DISPLAY_H
+#define INFO_DISPLAY_H
+
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include <rtabmap_rviz_plugins/visibility.h>
+#include <rtabmap_msgs/msg/info.hpp>
+
+#include <rviz_common/display.hpp>
+#include "rviz_common/message_filter_display.hpp"
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap_rviz_plugins
+{
+
+class RTABMAP_RVIZ_PLUGINS_PUBLIC InfoDisplay: public rviz_common::MessageFilterDisplay<rtabmap_msgs::msg::Info>
+{
+Q_OBJECT
+public:
+	InfoDisplay();
+	virtual ~InfoDisplay();
+
+	virtual void reset();
+	virtual void update( float wall_dt, float ros_dt );
+
+protected:
+	/** @brief Do initialization. Overridden from MessageFilterDisplay. */
+	virtual void onInitialize();
+
+	/** @brief Process a single message.  Overridden from MessageFilterDisplay. */
+	virtual void processMessage( const rtabmap_msgs::msg::Info::ConstSharedPtr cloud );
+
+private:
+	QString info_;
+	int globalCount_;
+	int localCount_;
+	std::map<std::string, float> statistics_;
+	rtabmap::Transform loopTransform_;
+	std::mutex info_mutex_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif

rtabmap_ros/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h

@@ -0,0 +1,221 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef MAP_CLOUD_DISPLAY_H
+#define MAP_CLOUD_DISPLAY_H
+
+#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
+
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include <rtabmap_rviz_plugins/visibility.h>
+#include <rtabmap_msgs/msg/map_data.hpp>
+#include <rtabmap/core/Transform.h>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include "rviz_common/message_filter_display.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_transformer.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_transformer_factory.hpp"
+#include <std_msgs/msg/int32_multi_array.hpp>
+
+#endif
+
+namespace rviz_common
+{
+
+namespace properties
+{
+
+class BoolProperty;
+class EnumProperty;
+class IntProperty;
+class FloatProperty;
+
+}  // namespace properties
+
+}  // namespace rviz_common
+
+namespace rtabmap_rviz_plugins
+{
+
+struct RTABMAP_RVIZ_PLUGINS_PUBLIC CloudInfo
+{
+	CloudInfo();
+	~CloudInfo();
+
+	// clear the point cloud, but keep selection handler around
+	void clear();
+
+	rclcpp::Time receive_time_;
+
+	Ogre::SceneManager *manager_;
+
+	sensor_msgs::msg::PointCloud2::ConstSharedPtr message_;
+	rtabmap::Transform pose_;
+	int id_;
+
+	Ogre::SceneNode *scene_node_;
+	std::shared_ptr<rviz_rendering::PointCloud> cloud_;
+	std::shared_ptr<rviz_default_plugins::PointCloudSelectionHandler> selection_handler_;
+
+	std::vector<rviz_rendering::PointCloud::Point> transformed_points_;
+};
+typedef std::shared_ptr<CloudInfo> CloudInfoPtr;
+
+/**
+ * \class MapCloudDisplay_
+ * \brief Displays point clouds from rtabmap::MapData
+ *
+ * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity.
+ * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b
+ * all being 8 bits.
+ */
+class RTABMAP_RVIZ_PLUGINS_PUBLIC MapCloudDisplay: public rviz_common::MessageFilterDisplay<rtabmap_msgs::msg::MapData>
+{
+Q_OBJECT
+public:
+	explicit MapCloudDisplay();
+	virtual ~MapCloudDisplay() {}
+
+	virtual void reset();
+	virtual void update( float wall_dt, float ros_dt );
+
+	bool auto_size_;
+
+	rviz_common::properties::FloatProperty* point_world_size_property_;
+	rviz_common::properties::FloatProperty* point_pixel_size_property_;
+	rviz_common::properties::FloatProperty* alpha_property_;
+	rviz_common::properties::EnumProperty* xyz_transformer_property_;
+	rviz_common::properties::EnumProperty* color_transformer_property_;
+	rviz_common::properties::EnumProperty* style_property_;
+	rviz_common::properties::BoolProperty* cloud_from_scan_;
+	rviz_common::properties::IntProperty* cloud_decimation_;
+	rviz_common::properties::FloatProperty* cloud_max_depth_;
+	rviz_common::properties::FloatProperty* cloud_min_depth_;
+	rviz_common::properties::FloatProperty* cloud_voxel_size_;
+	rviz_common::properties::FloatProperty* cloud_filter_floor_height_;
+	rviz_common::properties::FloatProperty* cloud_filter_ceiling_height_;
+	rviz_common::properties::FloatProperty* node_filtering_radius_;
+	rviz_common::properties::FloatProperty* node_filtering_angle_;
+	rviz_common::properties::StringProperty* download_namespace;
+	rviz_common::properties::BoolProperty* download_map_;
+	rviz_common::properties::BoolProperty* download_graph_;
+
+public Q_SLOTS:
+	void causeRetransform();
+
+private Q_SLOTS:
+	void updateStyle();
+	void updateBillboardSize();
+	void updateAlpha();
+	void updateXyzTransformer();
+	void updateColorTransformer();
+	void setXyzTransformerOptions( rviz_common::properties::EnumProperty* prop );
+	void setColorTransformerOptions( rviz_common::properties::EnumProperty* prop );
+	void updateCloudParameters();
+	void downloadNamespaceChanged();
+	void downloadMap();
+	void downloadGraph();
+
+protected:
+	/** @brief Process a single message.  Overridden from MessageFilterDisplay. */
+	virtual void processMessage( const rtabmap_msgs::msg::MapData::ConstSharedPtr cloud );
+	void onInitialize();
+private:
+	void downloadMap(bool graphOnly);
+	void processMapData(const rtabmap_msgs::msg::MapData& map);
+
+	/**
+	* \brief Transforms the cloud into the correct frame, and sets up our renderable cloud
+	*/
+	bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
+
+	std::shared_ptr<rviz_default_plugins::PointCloudTransformer> getXYZTransformer(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud);
+	std::shared_ptr<rviz_default_plugins::PointCloudTransformer> getColorTransformer(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud);
+	void updateTransformers( const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud );
+	void retransform();
+
+	void loadTransformers();
+	void loadTransformer(
+		std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans,
+	    std::string name,
+	    const std::string & lookup_name);
+
+	void setPropertiesHidden( const QList<rviz_common::properties::Property*>& props, bool hide );
+	void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask );
+
+private:
+	std::shared_ptr<rclcpp::Node> clientNode_;
+	rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataPub_;
+	
+	std::map<int, CloudInfoPtr> cloud_infos_;
+
+	std::map<int, CloudInfoPtr> new_cloud_infos_;
+	std::mutex new_clouds_mutex_;
+
+	std::set<int> nodeDataReceived_;
+	bool fromScan_;
+
+	std::map<int, rtabmap::Transform> current_map_;
+	std::mutex current_map_mutex_;
+	bool current_map_updated_;
+
+	int lastCloudAdded_;
+
+	struct TransformerInfo
+	{
+		std::shared_ptr<rviz_default_plugins::PointCloudTransformer> transformer;
+		QList<rviz_common::properties::Property*> xyz_props;
+		QList<rviz_common::properties::Property*> color_props;
+
+		std::string readable_name;
+		std::string lookup_name;
+	};
+	typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
+
+	std::recursive_mutex transformers_mutex_;
+	M_TransformerInfo transformers_;
+	bool new_xyz_transformer_;
+	bool new_color_transformer_;
+	bool needs_retransform_;
+
+	std::unique_ptr<rviz_default_plugins::PointCloudTransformerFactory> transformer_factory_;
+
+	rclcpp::Clock::SharedPtr clock_;
+
+	static const std::string message_status_name_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif

rtabmap_ros/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapGraphDisplay.h

@@ -0,0 +1,97 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#ifndef MAP_GRAPH_DISPLAY_H
+#define MAP_GRAPH_DISPLAY_H
+
+#include <rtabmap_rviz_plugins/visibility.h>
+#include <rtabmap_msgs/msg/map_graph.hpp>
+
+#include <rviz_common/message_filter_display.hpp>
+
+namespace Ogre
+{
+class ManualObject;
+}
+
+namespace rviz_common
+{
+
+namespace properties
+{
+
+class ColorProperty;
+class FloatProperty;
+
+}  // namespace properties
+
+}  // namespace rviz_common
+
+namespace rtabmap_rviz_plugins
+{
+
+/**
+ * \class MapGraphDisplay
+ * \brief Displays the graph of rtabmap::MapGraph message
+ */
+class RTABMAP_RVIZ_PLUGINS_PUBLIC MapGraphDisplay: public rviz_common::MessageFilterDisplay<rtabmap_msgs::msg::MapGraph>
+{
+Q_OBJECT
+public:
+  MapGraphDisplay();
+  virtual ~MapGraphDisplay();
+
+  /** @brief Overridden from Display. */
+  virtual void reset();
+
+protected:
+  /** @brief Overridden from Display. */
+  virtual void onInitialize();
+
+  /** @brief Overridden from MessageFilterDisplay. */
+  void processMessage( const rtabmap_msgs::msg::MapGraph::ConstSharedPtr msg );
+
+private:
+  void destroyObjects();
+
+  std::vector<Ogre::ManualObject*> manual_objects_;
+
+  rviz_common::properties::ColorProperty* color_neighbor_property_;
+  rviz_common::properties::ColorProperty* color_neighbor_merged_property_;
+  rviz_common::properties::ColorProperty* color_global_property_;
+  rviz_common::properties::ColorProperty* color_local_property_;
+  rviz_common::properties::ColorProperty* color_landmark_property_;
+  rviz_common::properties::ColorProperty* color_user_property_;
+  rviz_common::properties::ColorProperty* color_virtual_property_;
+  rviz_common::properties::FloatProperty* alpha_property_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif /* MAP_GRAPH_DISPLAY_H */
+

rtabmap_ros/rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/visibility.h

@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// 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 RTABMAP_RVIZ_PLUGINS__VISIBILITY_CONTROL_H_
+#define RTABMAP_RVIZ_PLUGINS__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RTABMAP_RVIZ_PLUGINS_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_RVIZ_PLUGINS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_RVIZ_PLUGINS_EXPORT __declspec(dllexport)
+    #define RTABMAP_RVIZ_PLUGINS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_RVIZ_PLUGINS_BUILDING_DLL
+    #define RTABMAP_RVIZ_PLUGINS_PUBLIC RTABMAP_RVIZ_PLUGINS_EXPORT
+  #else
+    #define RTABMAP_RVIZ_PLUGINS_PUBLIC RTABMAP_RVIZ_PLUGINS_IMPORT
+  #endif
+  #define RTABMAP_RVIZ_PLUGINS_PUBLIC_TYPE RTABMAP_RVIZ_PLUGINS_PUBLIC
+  #define RTABMAP_RVIZ_PLUGINS_LOCAL
+#else
+  #define RTABMAP_RVIZ_PLUGINS_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_RVIZ_PLUGINS_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_RVIZ_PLUGINS_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_RVIZ_PLUGINS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_RVIZ_PLUGINS_PUBLIC
+    #define RTABMAP_RVIZ_PLUGINS_LOCAL
+  #endif
+  #define RTABMAP_RVIZ_PLUGINS_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_RVIZ_PLUGINS__VISIBILITY_CONTROL_H_

rtabmap_ros/rtabmap_rviz_plugins/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_rviz_plugins</name>
+  <version>0.22.0</version>
+  <description>RTAB-Map's rviz plugins.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_depend>ros_environment</build_depend>
+
+  <depend>pcl_conversions</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_rendering</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>rtabmap_conversions</depend>
+  <depend>rtabmap_msgs</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+
+</package>

rtabmap_ros/rtabmap_rviz_plugins/rviz_plugins.xml

@@ -0,0 +1,26 @@
+<library path="rtabmap_rviz_plugins">
+  <class name="rtabmap_rviz_plugins/MapCloud"
+         type="rtabmap_rviz_plugins::MapCloudDisplay"
+         base_class_type="rviz_common::Display">
+    <description>
+      Displays graph point clouds from rtabmap_rviz_plugins/MapData messages.
+    </description>
+    <message_type>rtabmap_rviz_plugins/msg/MapData</message_type>
+  </class>
+  <class name="rtabmap_rviz_plugins/MapGraph"
+         type="rtabmap_rviz_plugins::MapGraphDisplay"
+         base_class_type="rviz_common::Display">
+    <description>
+      Displays graphs from rtabmap_rviz_plugins/MapGraph messages.
+    </description>
+    <message_type>rtabmap_rviz_plugins/msg/MapGraph</message_type>
+  </class>
+  <class name="rtabmap_rviz_plugins/Info"
+         type="rtabmap_rviz_plugins::InfoDisplay"
+         base_class_type="rviz_common::Display">
+    <description>
+      Displays information from rtabmap_rviz_plugins/Info messages.
+    </description>
+    <message_type>rtabmap_rviz_plugins/msg/Info</message_type>
+  </class>
+</library>

rtabmap_ros/rtabmap_rviz_plugins/src/InfoDisplay.cpp

@@ -0,0 +1,127 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_rviz_plugins/InfoDisplay.h"
+
+#include "rtabmap_conversions/MsgConversion.h"
+
+namespace rtabmap_rviz_plugins
+{
+
+InfoDisplay::InfoDisplay()
+  : globalCount_(0),
+    localCount_(0)
+{
+}
+
+InfoDisplay::~InfoDisplay()
+{
+}
+
+void InfoDisplay::onInitialize()
+{
+	MFDClass::onInitialize();
+
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Info", "");
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", "");
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", "0");
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", "0");
+}
+
+void InfoDisplay::processMessage( const rtabmap_msgs::msg::Info::ConstSharedPtr msg )
+{
+	{
+		std::unique_lock<std::mutex> lock(info_mutex_);
+		if(msg->loop_closure_id)
+		{
+			info_ = QString("%1->%2").arg(msg->ref_id).arg(msg->loop_closure_id);
+			globalCount_ += 1;
+		}
+		else if(msg->proximity_detection_id)
+		{
+			info_ = QString("%1->%2 [Proximity]").arg(msg->ref_id).arg(msg->proximity_detection_id);
+			localCount_ += 1;
+		}
+		else
+		{
+			info_ = "";
+		}
+		loopTransform_ = rtabmap_conversions::transformFromGeometryMsg(msg->loop_closure_transform);
+
+		rtabmap::Statistics stat;
+		rtabmap_conversions::infoFromROS(*msg, stat);
+		statistics_ = stat.data();
+	}
+
+
+	this->emitTimeSignal(msg->header.stamp);
+}
+
+void InfoDisplay::update( float /*wall_dt*/, float /*ros_dt*/ )
+{
+	{
+		std::unique_lock<std::mutex> lock(info_mutex_);
+		this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString());
+		if(loopTransform_.isNull())
+		{
+			this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
+			this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", "");
+		}
+		else
+		{
+			float x,y,z, roll,pitch,yaw;
+			loopTransform_.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
+			this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
+		}
+		this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
+		this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
+
+		for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
+		{
+			this->setStatus(rviz_common::properties::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
+		}
+	}
+}
+
+void InfoDisplay::reset()
+{
+	MFDClass::reset();
+	{
+		std::unique_lock<std::mutex> lock(info_mutex_);
+		info_.clear();
+		globalCount_ = 0;
+		localCount_ = 0;
+		statistics_.clear();
+	}
+}
+
+} // namespace rtabmap_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS( rtabmap_rviz_plugins::InfoDisplay, rviz_common::Display )

rtabmap_ros/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp

@@ -0,0 +1,978 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_rviz_plugins/MapCloudDisplay.h"
+
+#include <QApplication>
+#include <QMessageBox>
+#include <QTimer>
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+
+#include "rclcpp/clock.hpp"
+
+#include "rviz_common/display.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp"
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include "rviz_common/properties/bool_property.hpp"
+#include "rviz_common/properties/enum_property.hpp"
+#include "rviz_common/properties/float_property.hpp"
+#include "rviz_common/properties/vector_property.hpp"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap_msgs/srv/get_map.hpp>
+
+namespace rtabmap_rviz_plugins
+{
+
+
+CloudInfo::CloudInfo() :
+		manager_(nullptr),
+		pose_(rtabmap::Transform::getIdentity()),
+		id_(0),
+		scene_node_(nullptr)
+{}
+
+CloudInfo::~CloudInfo()
+{
+	clear();
+}
+
+void CloudInfo::clear()
+{
+	if ( scene_node_ )
+	{
+		manager_->destroySceneNode( scene_node_ );
+		scene_node_=nullptr;
+	}
+}
+
+const std::string MapCloudDisplay::message_status_name_ = "Message";  // NOLINT allow std::string
+
+MapCloudDisplay::MapCloudDisplay()
+  : auto_size_(false),
+	current_map_updated_(false),
+    lastCloudAdded_(-1),
+    new_xyz_transformer_(false),
+    new_color_transformer_(false),
+    needs_retransform_(false),
+	transformer_factory_(std::make_unique<rviz_default_plugins::PointCloudTransformerFactory>())
+{
+	//QIcon icon;
+	//this->setIcon(icon);
+
+	auto options = rclcpp::NodeOptions().arguments(
+		{"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"});
+	clientNode_ = std::make_shared<rclcpp::Node>("_", options);
+
+	style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares",
+										"Rendering mode to use, in order of computational complexity.",
+										this, SLOT( updateStyle() ), this );
+	style_property_->addOption( "Points", rviz_rendering::PointCloud::RM_POINTS );
+	style_property_->addOption( "Squares", rviz_rendering::PointCloud::RM_SQUARES );
+	style_property_->addOption( "Flat Squares", rviz_rendering::PointCloud::RM_FLAT_SQUARES );
+	style_property_->addOption( "Spheres", rviz_rendering::PointCloud::RM_SPHERES );
+	style_property_->addOption( "Boxes", rviz_rendering::PointCloud::RM_BOXES );
+
+	point_world_size_property_ = new rviz_common::properties::FloatProperty( "Size (m)", 0.01,
+												  "Point size in meters.",
+												  this, SLOT( updateBillboardSize() ), this );
+	point_world_size_property_->setMin( 0.0001 );
+
+	point_pixel_size_property_ = new rviz_common::properties::FloatProperty( "Size (Pixels)", 3,
+												  "Point size in pixels.",
+												  this, SLOT( updateBillboardSize() ), this );
+	point_pixel_size_property_->setMin( 1 );
+
+	alpha_property_ = new rviz_common::properties::FloatProperty( "Alpha", 1.0,
+										 "Amount of transparency to apply to the points.  Note that this is experimental and does not always look correct.",
+										 this, SLOT( updateAlpha() ), this );
+	alpha_property_->setMin( 0 );
+	alpha_property_->setMax( 1 );
+
+	xyz_transformer_property_ = new rviz_common::properties::EnumProperty( "Position Transformer", "",
+												  "Set the transformer to use to set the position of the points.",
+												  this, SLOT( updateXyzTransformer() ), this );
+	connect( xyz_transformer_property_, SIGNAL( requestOptions( rviz_common::properties::EnumProperty* )),
+			 this, SLOT( setXyzTransformerOptions( rviz_common::properties::EnumProperty* )));
+
+	color_transformer_property_ = new rviz_common::properties::EnumProperty( "Color Transformer", "",
+													"Set the transformer to use to set the color of the points.",
+													this, SLOT( updateColorTransformer() ), this );
+	connect( color_transformer_property_, SIGNAL( requestOptions( rviz_common::properties::EnumProperty* )),
+			 this, SLOT( setColorTransformerOptions( rviz_common::properties::EnumProperty* )));
+
+	cloud_from_scan_ = new rviz_common::properties::BoolProperty( "Cloud from scan", false,
+										 "Create the cloud from laser scans instead of the RGB-D/Stereo images.",
+										 this, SLOT( updateCloudParameters() ), this );
+	fromScan_ = cloud_from_scan_->getBool();
+
+	cloud_decimation_ = new rviz_common::properties::IntProperty( "Cloud decimation", 4,
+										 "Decimation of the input RGB and depth images before creating the cloud.",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_decimation_->setMin( 1 );
+	cloud_decimation_->setMax( 16 );
+
+	cloud_max_depth_ = new rviz_common::properties::FloatProperty( "Cloud max depth (m)", 4.0f,
+										 "Maximum depth of the generated clouds.",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_max_depth_->setMin( 0.0f );
+	cloud_max_depth_->setMax( 999.0f );
+
+	cloud_min_depth_ = new rviz_common::properties::FloatProperty( "Cloud min depth (m)", 0.0f,
+											 "Minimum depth of the generated clouds.",
+											 this, SLOT( updateCloudParameters() ), this );
+	cloud_min_depth_->setMin( 0.0f );
+	cloud_min_depth_->setMax( 999.0f );
+
+	cloud_voxel_size_ = new rviz_common::properties::FloatProperty( "Cloud voxel size (m)", 0.01f,
+										 "Voxel size of the generated clouds.",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_voxel_size_->setMin( 0.0f );
+	cloud_voxel_size_->setMax( 1.0f );
+
+	cloud_filter_floor_height_ = new rviz_common::properties::FloatProperty( "Filter floor (m)", 0.0f,
+										 "Filter the floor up to maximum height set here "
+										 "(only appropriate for 2D mapping).",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_filter_floor_height_->setMin( -999.0f );
+	cloud_filter_floor_height_->setMax( 999.0f );
+
+	cloud_filter_ceiling_height_ = new rviz_common::properties::FloatProperty( "Filter ceiling (m)", 0.0f,
+										 "Filter the ceiling at the specified height set here "
+										 "(only appropriate for 2D mapping).",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_filter_ceiling_height_->setMin( -999.0f );
+	cloud_filter_ceiling_height_->setMax( 999.0f );
+
+	node_filtering_radius_ = new rviz_common::properties::FloatProperty( "Node filtering radius (m)", 0.0f,
+										 "(Disabled=0) Only keep one node in the specified radius.",
+										 this, SLOT( updateCloudParameters() ), this );
+	node_filtering_radius_->setMin( 0.0f );
+	node_filtering_radius_->setMax( 10.0f );
+
+	node_filtering_angle_ = new rviz_common::properties::FloatProperty( "Node filtering angle (degrees)", 30.0f,
+										 "(Disabled=0) Only keep one node in the specified angle in the filtering radius.",
+										 this, SLOT( updateCloudParameters() ), this );
+	node_filtering_angle_->setMin( 0.0f );
+	node_filtering_angle_->setMax( 359.0f );
+
+	download_namespace = new rviz_common::properties::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
+
+	download_map_ = new rviz_common::properties::BoolProperty( "Download map", false,
+										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
+										 this, SLOT( downloadMap() ), this );
+
+	download_graph_ = new rviz_common::properties::BoolProperty( "Download graph", false,
+											 "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
+											 this, SLOT( downloadGraph() ), this );
+}
+
+
+void MapCloudDisplay::onInitialize()
+{
+    MFDClass::onInitialize();
+
+	loadTransformers();
+
+	updateStyle();
+	updateBillboardSize();
+	updateAlpha();
+
+	downloadNamespaceChanged();
+}
+
+void MapCloudDisplay::loadTransformers()
+{
+	auto plugins = transformer_factory_->getDeclaredPlugins();
+	  for (auto const & plugin : plugins) {
+	    auto plugin_name_std = plugin.name.toStdString();
+	    if (transformers_.count(plugin_name_std) > 0) {
+	      RVIZ_COMMON_LOG_ERROR_STREAM("Transformer type " << plugin_name_std << " is already loaded.");
+	      continue;
+	    }
+
+	    std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans(transformer_factory_->make(plugin.id));
+	    loadTransformer(trans, plugin_name_std, plugin.id.toStdString());
+	  }
+}
+
+void MapCloudDisplay::loadTransformer(
+  std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans,
+  std::string name,
+  const std::string & lookup_name)
+{
+  trans->init();
+  connect(trans.get(), SIGNAL(needRetransform()), this, SLOT(causeRetransform()));
+
+  TransformerInfo info;
+  info.transformer = trans;
+  info.readable_name = name;
+  info.lookup_name = lookup_name;
+
+  info.transformer->createProperties(
+    this, rviz_default_plugins::PointCloudTransformer::Support_XYZ, info.xyz_props);
+  setPropertiesHidden(info.xyz_props, true);
+
+  info.transformer->createProperties(
+    this, rviz_default_plugins::PointCloudTransformer::Support_Color, info.color_props);
+  setPropertiesHidden(info.color_props, true);
+
+  transformers_[name] = info;
+}
+
+void MapCloudDisplay::processMessage( const rtabmap_msgs::msg::MapData::ConstSharedPtr msg )
+{
+	processMapData(*msg);
+
+	this->emitTimeSignal(msg->header.stamp);
+}
+
+void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
+{
+	std::map<int, rtabmap::Transform> poses;
+	for(unsigned int i=0; i<map.graph.poses_id.size() && i<map.graph.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(map.graph.poses_id[i], rtabmap_conversions::transformFromPoseMsg(map.graph.poses[i])));
+	}
+
+	// Add new clouds...
+	bool fromDepth = !cloud_from_scan_->getBool();
+	std::set<int> nodeDataReceived;
+	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
+	{
+		int id = map.nodes[i].id;
+
+		// Always refresh the cloud if there are data
+		rtabmap::Signature s = rtabmap_conversions::nodeFromROS(map.nodes[i]);
+		if((fromDepth &&
+			!s.sensorData().imageCompressed().empty() &&
+		    !s.sensorData().depthOrRightCompressed().empty() &&
+		    (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModels().size())) ||
+		   (!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
+		{
+			cv::Mat image, depth;
+			rtabmap::LaserScan scan;
+
+			s.sensorData().uncompressData(fromDepth?&image:0, fromDepth?&depth:0, !fromDepth?&scan:0);
+
+			sensor_msgs::msg::PointCloud2::SharedPtr cloudMsg(new sensor_msgs::msg::PointCloud2);
+			if(fromDepth && !s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+				pcl::IndicesPtr validIndices(new std::vector<int>);
+
+				cloud = rtabmap::util3d::cloudRGBFromSensorData(
+						s.sensorData(),
+						cloud_decimation_->getInt(),
+						cloud_max_depth_->getFloat(),
+						cloud_min_depth_->getFloat(),
+						validIndices.get());
+
+				if(!cloud->empty())
+				{
+					if(cloud_voxel_size_->getFloat())
+					{
+						cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
+					}
+
+					if(cloud_filter_floor_height_->getFloat() != 0.0f || cloud_filter_ceiling_height_->getFloat() != 0.0f)
+					{
+						// convert in /odom frame
+						cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
+						cloud = rtabmap::util3d::passThrough(cloud, "z",
+								cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+								cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
+						// convert back in /base_link frame
+						if(!cloud->empty())
+							cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
+					}
+
+					if(!cloud->empty())
+					{
+						pcl::toROSMsg(*cloud, *cloudMsg);
+					}
+				}
+			}
+			else if(!fromDepth && !scan.isEmpty())
+			{
+				scan = rtabmap::util3d::commonFiltering(
+						scan,
+						1,
+						cloud_min_depth_->getFloat(),
+						cloud_max_depth_->getFloat(),
+						cloud_voxel_size_->getFloat());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
+				cloud = rtabmap::util3d::laserScanToPointCloudI(scan, scan.localTransform());
+				if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
+				{
+					// convert in /odom frame
+					cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
+					cloud = rtabmap::util3d::passThrough(cloud, "z",
+							cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+							cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
+					// convert back in /base_link frame
+					cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
+				}
+
+				if(!cloud->empty())
+				{
+					pcl::toROSMsg(*cloud, *cloudMsg);
+				}
+			}
+
+			if(!cloudMsg->data.empty())
+			{
+				cloudMsg->header = map.header;
+				CloudInfoPtr info(new CloudInfo);
+				info->message_ = cloudMsg;
+				info->pose_ = rtabmap::Transform::getIdentity();
+				info->id_ = id;
+
+				if (transformCloud(info, true))
+				{
+					std::unique_lock<std::mutex> lock(new_clouds_mutex_);
+					new_cloud_infos_.erase(id);
+					new_cloud_infos_.insert(std::make_pair(id, info));
+				}
+			}
+		}
+		nodeDataReceived.insert(id);
+	}
+
+	// Update graph
+	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
+	{
+		poses = rtabmap::graph::radiusPosesFiltering(poses,
+				node_filtering_radius_->getFloat(),
+				node_filtering_angle_->getFloat()*CV_PI/180.0);
+	}
+
+	{
+		std::unique_lock<std::mutex> lock(current_map_mutex_);
+		current_map_ = poses;
+		current_map_updated_ = true;
+		nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
+	}
+}
+
+void MapCloudDisplay::setPropertiesHidden( const QList<rviz_common::properties::Property*>& props, bool hide )
+{
+  for (auto prop : props) {
+	prop->setHidden(hide);
+  }
+}
+
+void MapCloudDisplay::updateTransformers( const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud )
+{
+  std::string xyz_name = xyz_transformer_property_->getStdString();
+  std::string color_name = color_transformer_property_->getStdString();
+
+  xyz_transformer_property_->clearOptions();
+  color_transformer_property_->clearOptions();
+
+  // Get the channels that we could potentially render
+  typedef std::set<std::pair<uint8_t, std::string>> S_string;
+  S_string valid_xyz, valid_color;
+  bool cur_xyz_valid = false;
+  bool cur_color_valid = false;
+  bool has_rgb_transformer = false;
+  for (auto transformer : transformers_) {
+    const std::string & name = transformer.first;
+    const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = transformer.second.transformer;
+    uint32_t mask = trans->supports(cloud);
+    if (mask & rviz_default_plugins::PointCloudTransformer::Support_XYZ) {
+      valid_xyz.insert(std::make_pair(trans->score(cloud), name));
+      if (name == xyz_name) {
+        cur_xyz_valid = true;
+      }
+      xyz_transformer_property_->addOptionStd(name);
+    }
+
+    if (mask & rviz_default_plugins::PointCloudTransformer::Support_Color) {
+      valid_color.insert(std::make_pair(trans->score(cloud), name));
+      if (name == color_name) {
+        cur_color_valid = true;
+      }
+      if (name == "RGB8") {
+        has_rgb_transformer = true;
+      }
+      color_transformer_property_->addOptionStd(name);
+    }
+  }
+
+  if (!cur_xyz_valid) {
+    if (!valid_xyz.empty()) {
+      xyz_transformer_property_->setStringStd(valid_xyz.rbegin()->second);
+    }
+  }
+
+  if (!cur_color_valid) {
+    if (!valid_color.empty()) {
+      if (has_rgb_transformer) {
+        color_transformer_property_->setStringStd("RGB8");
+      } else {
+        color_transformer_property_->setStringStd(valid_color.rbegin()->second);
+      }
+    }
+  }
+}
+
+void MapCloudDisplay::updateAlpha()
+{
+	for (auto const & cloud_info : cloud_infos_) {
+	    bool per_point_alpha = rviz_default_plugins::findChannelIndex(cloud_info.second->message_, "rgba") != -1;
+	    cloud_info.second->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
+	  }
+}
+
+void MapCloudDisplay::updateStyle()
+{
+  auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
+  if (mode == rviz_rendering::PointCloud::RM_POINTS) {
+    point_world_size_property_->hide();
+    point_pixel_size_property_->show();
+  } else {
+    point_world_size_property_->show();
+    point_pixel_size_property_->hide();
+  }
+  for (auto const & cloud_info : cloud_infos_) {
+    cloud_info.second->cloud_->setRenderMode(mode);
+  }
+  updateBillboardSize();
+}
+
+void MapCloudDisplay::updateBillboardSize()
+{
+  auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
+  float size;
+  if (mode == rviz_rendering::PointCloud::RM_POINTS) {
+    size = point_pixel_size_property_->getFloat();
+  } else {
+    size = point_world_size_property_->getFloat();
+  }
+  for (auto & cloud_info : cloud_infos_) {
+    cloud_info.second->cloud_->setDimensions(size, size, size);
+  }
+  context_->queueRender();
+}
+
+void MapCloudDisplay::updateCloudParameters()
+{
+	// do nothing for most parameters... only take effect on next generated clouds
+
+	// if we change the kind of map, clear
+	if(fromScan_ != cloud_from_scan_->getBool())
+	{
+		reset();
+	}
+	fromScan_ = cloud_from_scan_->getBool();
+}
+
+void MapCloudDisplay::downloadMap(bool graphOnly)
+{
+	auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+	request->global_map = false;
+	request->optimized = true;
+	request->graph_only = graphOnly;
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string srvName = rtabmapNs+"/get_map_data";
+	QMessageBox * messageBox = new QMessageBox(
+			QMessageBox::NoIcon,
+			tr("Calling \"%1\" service...").arg(srvName.c_str()),
+			tr("Downloading the map... please wait (rviz could become gray!)"),
+			QMessageBox::NoButton,
+			getAssociatedWidget());
+	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+	messageBox->show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
+	
+	RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str()));
+	
+	auto client = clientNode_->create_client<rtabmap_msgs::srv::GetMap>(srvName);
+	if(client->wait_for_service(std::chrono::seconds(1)))
+	{
+		RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str()));
+		auto result = client->async_send_request(request);
+		if (rclcpp::spin_until_future_complete(clientNode_, result) ==
+    		rclcpp::FutureReturnCode::SUCCESS)
+		{
+			RVIZ_COMMON_LOG_INFO(uFormat("Process data"));
+			auto future = result.get();
+			if(graphOnly)
+			{
+				messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size()));
+				QApplication::processEvents();
+				processMapData(future->data);
+				messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size()));
+				QApplication::processEvents();
+				QTimer::singleShot(1000, messageBox, SLOT(close()));
+			}
+			else
+			{
+				messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
+						.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
+				QApplication::processEvents();
+				this->reset();
+				processMapData(future->data);
+				messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
+						.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
+
+				QTimer::singleShot(1000, messageBox, SLOT(close()));
+			}
+		} else {
+			std::string msg = uFormat("Failed to call service %s", srvName.c_str());
+			RVIZ_COMMON_LOG_ERROR(msg);
+			messageBox->setText(msg.c_str());
+		}
+	}
+	else
+	{
+		std::string msg = uFormat("MapCloudDisplay: Cannot call \"%s\" service. "
+				  "Tip: if rtabmap node is not in \"%s\" namespace, you can "
+				  "change the \"Download namespace\" option.",
+				  srvName.c_str(),
+				  rtabmapNs.c_str());
+		RVIZ_COMMON_LOG_ERROR(msg);
+		messageBox->setText(msg.c_str());
+	}
+}
+
+void MapCloudDisplay::downloadNamespaceChanged()
+{
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string topicName = uFormat("%s/republish_node_data", rtabmapNs.c_str());
+	republishNodeDataPub_ = rviz_ros_node_.lock()->get_raw_node()->create_publisher<std_msgs::msg::Int32MultiArray>(topicName, 1);
+}
+
+void MapCloudDisplay::downloadMap()
+{
+	if(download_map_->getBool())
+	{
+		downloadMap(false);
+		download_map_->blockSignals(true);
+		download_map_->setBool(false);
+		download_map_->blockSignals(false);
+	}
+	else
+	{
+		// just stay true if double-clicked on DownloadMap property, let the
+		// first process above finishes
+		download_map_->blockSignals(true);
+		download_map_->setBool(true);
+		download_map_->blockSignals(false);
+	}
+}
+
+void MapCloudDisplay::downloadGraph()
+{
+	if(download_graph_->getBool())
+	{
+		downloadMap(true);
+		download_graph_->blockSignals(true);
+		download_graph_->setBool(false);
+		download_graph_->blockSignals(false);
+	}
+	else
+	{
+		// just stay true if double-clicked on DownloadGraph property, let the
+		// first process above finishes
+		download_graph_->blockSignals(true);
+		download_graph_->setBool(true);
+		download_graph_->blockSignals(false);
+	}
+}
+
+void MapCloudDisplay::causeRetransform()
+{
+  needs_retransform_ = true;
+}
+
+void MapCloudDisplay::update( float, float )
+{
+	auto mode = static_cast<rviz_rendering::PointCloud::RenderMode>(style_property_->getOptionInt());
+
+	int lastCloudAdded = -1;
+
+	if (needs_retransform_)
+	{
+		retransform();
+		needs_retransform_ = false;
+	}
+
+	{
+		std::unique_lock<std::mutex> lock(new_clouds_mutex_);
+		if( !new_cloud_infos_.empty() )
+		{
+			float size;
+			  if (mode == rviz_rendering::PointCloud::RM_POINTS) {
+			    size = point_pixel_size_property_->getFloat();
+			  } else {
+			    size = point_world_size_property_->getFloat();
+			  }
+
+			auto it = new_cloud_infos_.begin();
+			auto end = new_cloud_infos_.end();
+			for (; it != end; ++it)
+			{
+				CloudInfoPtr cloud_info = it->second;
+
+				bool per_point_alpha = rviz_default_plugins::findChannelIndex(cloud_info->message_, "rgba") != -1;
+
+				cloud_info->cloud_.reset( new rviz_rendering::PointCloud() );
+				cloud_info->cloud_->addPoints(cloud_info->transformed_points_.begin(), cloud_info->transformed_points_.end());
+				cloud_info->cloud_->setRenderMode( mode );
+				cloud_info->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha);
+				cloud_info->cloud_->setDimensions( size, size, size );
+				cloud_info->cloud_->setAutoSize(auto_size_);
+
+				cloud_info->manager_ = context_->getSceneManager();
+
+				cloud_info->scene_node_ = scene_node_->createChildSceneNode();
+
+				cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
+				cloud_info->scene_node_->setVisible(false);
+
+				cloud_infos_.erase(it->first);
+				cloud_infos_.insert(*it);
+				lastCloudAdded = it->first;
+			}
+
+			new_cloud_infos_.clear();
+		}
+	}
+
+	{
+		std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
+
+		if( new_xyz_transformer_ || new_color_transformer_ )
+		{
+			for (auto transformer : transformers_) {
+			      const std::string & name = transformer.first;
+			      TransformerInfo & info = transformer.second;
+
+			      setPropertiesHidden(info.xyz_props, name != xyz_transformer_property_->getStdString());
+			      setPropertiesHidden(info.color_props,
+			        name != color_transformer_property_->getStdString());
+
+			      if (name == xyz_transformer_property_->getStdString() ||
+			        name == color_transformer_property_->getStdString())
+			      {
+			        info.transformer->hideUnusedProperties();
+			      }
+			    }
+		}
+
+		new_xyz_transformer_ = false;
+		new_color_transformer_ = false;
+	}
+
+	int totalPoints = 0;
+	int totalNodesShown = 0;
+	{
+		// update poses
+		std::unique_lock<std::mutex> lock(current_map_mutex_);
+		if(!current_map_.empty())
+		{
+			std::vector<int> missingNodes;
+			for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
+			{
+				std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
+				if(cloudInfoIt != cloud_infos_.end())
+				{
+					totalPoints += cloudInfoIt->second->transformed_points_.size();
+					cloudInfoIt->second->pose_ = it->second;
+					Ogre::Vector3 framePosition;
+					Ogre::Quaternion frameOrientation;
+					std::string error;
+					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, framePosition, frameOrientation))
+					{
+						// Multiply frame with pose
+						Ogre::Matrix4 frameTransform;
+						frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation);
+						const rtabmap::Transform & p = cloudInfoIt->second->pose_;
+						Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
+										 p[4], p[5], p[6], p[7],
+										 p[8], p[9], p[10], p[11],
+										 0, 0, 0, 1);
+						frameTransform = frameTransform * pose;
+						Ogre::Vector3 posePosition = frameTransform.getTrans();
+						Ogre::Quaternion poseOrientation(frameTransform.linear());
+						poseOrientation.normalise();
+
+						cloudInfoIt->second->scene_node_->setPosition(posePosition);
+						cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
+						cloudInfoIt->second->scene_node_->setVisible(true);
+						++totalNodesShown;
+					}
+					else if(context_->getFrameManager()->transformHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
+					{
+						RVIZ_COMMON_LOG_ERROR(uFormat("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\" (reason=%s), set fixed frame in global options to \"%s\")",
+								it->first,
+								cloudInfoIt->second->message_->header.frame_id.c_str(),
+								error.c_str(),
+								cloudInfoIt->second->message_->header.frame_id.c_str()));
+					}
+				}
+				else if(it->first>0 && current_map_updated_&& nodeDataReceived_.find(it->first) == nodeDataReceived_.end())
+				{
+					missingNodes.push_back(it->first);
+				}
+			}
+			//hide not used clouds
+			for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end();)
+			{
+				if(current_map_.find(iter->first) == current_map_.end())
+				{
+					if(iter->first == lastCloudAdded_)
+					{
+						// remove from cache, the node has been discarded
+						cloud_infos_.erase(iter++);
+						lastCloudAdded_ = -1;
+					}
+					else
+					{
+						iter->second->scene_node_->setVisible(false);
+						++iter;
+					}
+				}
+				else
+				{
+					++iter;
+				}
+			}
+
+			if(!missingNodes.empty() && republishNodeDataPub_.get())
+			{
+				std_msgs::msg::Int32MultiArray::UniquePtr msg(new std_msgs::msg::Int32MultiArray);
+				msg->data = missingNodes;
+				republishNodeDataPub_->publish(std::move(msg));
+			}
+		}
+		current_map_updated_ = false;
+	}
+	if(lastCloudAdded>0)
+	{
+		lastCloudAdded_ = lastCloudAdded;
+	}
+
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
+	this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
+}
+
+void MapCloudDisplay::reset()
+{
+	lastCloudAdded_ = -1;
+	{
+		std::unique_lock<std::mutex> lock(new_clouds_mutex_);
+		cloud_infos_.clear();
+		new_cloud_infos_.clear();
+	}
+	{
+		std::unique_lock<std::mutex> lock(current_map_mutex_);
+		current_map_.clear();
+		current_map_updated_ = false;
+		nodeDataReceived_.clear();
+	}
+}
+
+void MapCloudDisplay::updateXyzTransformer()
+{
+	std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
+	if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
+	{
+		return;
+	}
+	new_xyz_transformer_ = true;
+	causeRetransform();
+}
+
+void MapCloudDisplay::updateColorTransformer()
+{
+	std::unique_lock<std::recursive_mutex> lock( transformers_mutex_ );
+	if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
+	{
+		return;
+	}
+	new_color_transformer_ = true;
+	causeRetransform();
+}
+
+void MapCloudDisplay::setXyzTransformerOptions( rviz_common::properties::EnumProperty* prop )
+{
+	fillTransformerOptions( prop, rviz_default_plugins::PointCloudTransformer::Support_XYZ );
+}
+
+void MapCloudDisplay::setColorTransformerOptions( rviz_common::properties::EnumProperty* prop )
+{
+	fillTransformerOptions( prop, rviz_default_plugins::PointCloudTransformer::Support_Color );
+}
+
+void MapCloudDisplay::fillTransformerOptions(
+  rviz_common::properties::EnumProperty * prop,
+  uint32_t mask)
+{
+  prop->clearOptions();
+
+  if (cloud_infos_.empty()) {
+    return;
+  }
+
+  std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
+
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg = cloud_infos_.begin()->second->message_;
+
+  for (auto transformer : transformers_) {
+    const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = transformer.second.transformer;
+    if ((trans->supports(msg) & mask) == mask) {
+      prop->addOption(QString::fromStdString(transformer.first));
+    }
+  }
+}
+
+std::shared_ptr<rviz_default_plugins::PointCloudTransformer> MapCloudDisplay::getXYZTransformer(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & cloud)
+{
+  std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
+  auto it = transformers_.find(xyz_transformer_property_->getStdString());
+  if (it != transformers_.end()) {
+    const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = it->second.transformer;
+    if (trans->supports(cloud) & rviz_default_plugins::PointCloudTransformer::Support_XYZ) {
+      return trans;
+    }
+  }
+
+  return std::shared_ptr<rviz_default_plugins::PointCloudTransformer>();
+}
+
+std::shared_ptr<rviz_default_plugins::PointCloudTransformer> MapCloudDisplay::getColorTransformer(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & cloud)
+{
+  std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
+  auto it = transformers_.find(color_transformer_property_->getStdString());
+  if (it != transformers_.end()) {
+    const std::shared_ptr<rviz_default_plugins::PointCloudTransformer> & trans = it->second.transformer;
+    if (trans->supports(cloud) & rviz_default_plugins::PointCloudTransformer::Support_Color) {
+      return trans;
+    }
+  }
+
+  return std::shared_ptr<rviz_default_plugins::PointCloudTransformer>();
+}
+
+
+void MapCloudDisplay::retransform()
+{
+  std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
+
+  for (auto const & cloud_info : cloud_infos_) {
+    transformCloud(cloud_info.second, false);
+    cloud_info.second->cloud_->clear();
+    cloud_info.second->cloud_->addPoints(
+      cloud_info.second->transformed_points_.begin(), cloud_info.second->transformed_points_.end());
+  }
+}
+
+bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
+{
+	this->deleteStatusStd(message_status_name_);
+
+	rviz_default_plugins::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
+	cloud_points.clear();
+
+	size_t size = cloud_info->message_->width * cloud_info->message_->height;
+	rviz_rendering::PointCloud::Point default_pt;
+	default_pt.color = Ogre::ColourValue(1, 1, 1);
+	default_pt.position = Ogre::Vector3::ZERO;
+	cloud_points.resize(size, default_pt);
+
+	{
+		std::unique_lock<std::recursive_mutex> lock(transformers_mutex_);
+		if( update_transformers )
+		{
+			updateTransformers( cloud_info->message_ );
+		}
+		std::shared_ptr<rviz_default_plugins::PointCloudTransformer> xyz_trans = getXYZTransformer(cloud_info->message_);
+		std::shared_ptr<rviz_default_plugins::PointCloudTransformer> color_trans = getColorTransformer(cloud_info->message_);
+
+		if (cloud_info->message_->data.size() !=
+		    cloud_info->message_->width * cloud_info->message_->height * cloud_info->message_->point_step)
+		  {
+		    std::string status = "PointCloud contained not enough or too much data";
+		    this->setStatusStd(
+		      rviz_common::properties::StatusProperty::Error, message_status_name_, status);
+		    return false;
+		  }
+
+		if (!xyz_trans)
+		{
+			std::stringstream ss;
+			ss << "No position transformer available for cloud";
+			this->setStatusStd(rviz_common::properties::StatusProperty::Error, message_status_name_, ss.str());
+			return false;
+		}
+
+		if (!color_trans)
+		{
+			std::stringstream ss;
+			ss << "No color transformer available for cloud";
+			this->setStatusStd(rviz_common::properties::StatusProperty::Error, message_status_name_, ss.str());
+			return false;
+		}
+
+		xyz_trans->transform(cloud_info->message_, rviz_default_plugins::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
+		color_trans->transform(cloud_info->message_, rviz_default_plugins::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
+	}
+
+	for (auto & cloud_point : cloud_points) {
+	    if (!rviz_common::validateFloats(cloud_point.position)) {
+	      cloud_point.position.x = 999999.0f;
+	      cloud_point.position.y = 999999.0f;
+	      cloud_point.position.z = 999999.0f;
+	    }
+	  }
+
+	return true;
+}
+
+} // namespace rtabmap_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rtabmap_rviz_plugins::MapCloudDisplay, rviz_common::Display)

rtabmap_ros/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp

@@ -0,0 +1,178 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_rviz_plugins/MapGraphDisplay.h"
+#include <rviz_common/display_context.hpp>
+#include "rviz_common/properties/color_property.hpp"
+#include "rviz_common/properties/float_property.hpp"
+#include "rviz_common/properties/int_property.hpp"
+#include "rviz_common/logging.hpp"
+
+#include <rtabmap/core/Link.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+namespace rtabmap_rviz_plugins
+{
+
+MapGraphDisplay::MapGraphDisplay()
+{
+	color_neighbor_property_ = new rviz_common::properties::ColorProperty( "Neighbor", Qt::blue,
+	                                       "Color to draw neighbor links.", this );
+	color_neighbor_merged_property_ = new rviz_common::properties::ColorProperty( "Merged neighbor", QColor(255,170,0),
+	                                       "Color to draw merged neighbor links.", this );
+	color_global_property_ = new rviz_common::properties::ColorProperty( "Global loop closure", Qt::red,
+	                                       "Color to draw global loop closure links.", this );
+	color_local_property_ = new rviz_common::properties::ColorProperty( "Local loop closure", Qt::yellow,
+	                                       "Color to draw local loop closure links.", this );
+	color_landmark_property_ = new rviz_common::properties::ColorProperty( "Landmark", Qt::darkGreen,
+	                                       "Color to draw landmark links.", this );
+	color_user_property_ = new rviz_common::properties::ColorProperty( "User", Qt::red,
+	                                       "Color to draw user links.", this );
+	color_virtual_property_ = new rviz_common::properties::ColorProperty( "Virtual", Qt::magenta,
+	                                       "Color to draw virtual links.", this );
+
+	alpha_property_ = new rviz_common::properties::FloatProperty( "Alpha", 1.0,
+                                       "Amount of transparency to apply to the path.", this );
+}
+
+MapGraphDisplay::~MapGraphDisplay()
+{
+	destroyObjects();
+}
+
+void MapGraphDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  destroyObjects();
+}
+
+void MapGraphDisplay::reset()
+{
+  MFDClass::reset();
+  destroyObjects();
+}
+
+void MapGraphDisplay::destroyObjects()
+{
+	for(unsigned int i=0; i<manual_objects_.size(); ++i)
+	{
+		manual_objects_[i]->clear();
+		scene_manager_->destroyManualObject( manual_objects_[i] );
+	}
+	manual_objects_.clear();
+}
+
+void MapGraphDisplay::processMessage( const rtabmap_msgs::msg::MapGraph::ConstSharedPtr msg )
+{
+	if(!(msg->poses.size() == msg->poses_id.size()))
+	{
+		RVIZ_COMMON_LOG_ERROR("rtabmap_rviz_plugins::MapGraph: Error pose ids and poses must have all the same size.");
+		return;
+	}
+
+	// Get links
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> links;
+	rtabmap::Transform mapToOdom;
+	rtabmap_conversions::mapGraphFromROS(*msg, poses, links, mapToOdom);
+
+	destroyObjects();
+
+	Ogre::Vector3 position;
+	Ogre::Quaternion orientation;
+	if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
+	{
+		RVIZ_COMMON_LOG_ERROR( uFormat("Error transforming from frame '%s' to frame '%s'",
+				msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )));
+	}
+
+	Ogre::Matrix4 transform( orientation );
+	transform.setTrans( position );
+
+	if(links.size())
+	{
+		Ogre::ColourValue color;
+		Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
+		manual_object->setDynamic( true );
+		scene_node_->attachObject( manual_object );
+		manual_objects_.push_back(manual_object);
+
+		manual_object->estimateVertexCount(links.size() * 2);
+		manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
+		for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			std::map<int, rtabmap::Transform>::iterator poseIterFrom = poses.find(iter->second.from());
+			std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to());
+			if(poseIterFrom != poses.end() && poseIterTo != poses.end())
+			{
+				if(iter->second.type() == rtabmap::Link::kNeighbor)
+				{
+					color = color_neighbor_property_->getOgreColor();
+				}
+				else if(iter->second.type() == rtabmap::Link::kNeighborMerged)
+				{
+					color = color_neighbor_merged_property_->getOgreColor();
+				}
+				else if(iter->second.type() == rtabmap::Link::kVirtualClosure)
+				{
+					color = color_virtual_property_->getOgreColor();
+				}
+				else if(iter->second.type() == rtabmap::Link::kUserClosure)
+				{
+					color = color_user_property_->getOgreColor();
+				}
+				else if(iter->second.type() == rtabmap::Link::kLocalSpaceClosure || iter->second.type() == rtabmap::Link::kLocalTimeClosure)
+				{
+					color = color_local_property_->getOgreColor();
+				}
+				else if(iter->second.type() == rtabmap::Link::kLandmark)
+				{
+					color = color_landmark_property_->getOgreColor();
+				}
+				else
+				{
+					color = color_global_property_->getOgreColor();
+				}
+				color.a = alpha_property_->getFloat();
+				Ogre::Vector3 pos;
+				pos = transform * Ogre::Vector3( poseIterFrom->second.x(), poseIterFrom->second.y(), poseIterFrom->second.z() );
+				manual_object->position( pos.x, pos.y, pos.z );
+				manual_object->colour( color );
+				pos = transform * Ogre::Vector3( poseIterTo->second.x(), poseIterTo->second.y(), poseIterTo->second.z() );
+				manual_object->position( pos.x, pos.y, pos.z );
+				manual_object->colour( color );
+			}
+		}
+
+		manual_object->end();
+	}
+}
+
+} // namespace rtabmap_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS( rtabmap_rviz_plugins::MapGraphDisplay, rviz_common::Display )