First version rtabmap_launch working

rtabmap_rviz_plugins/CMakeLists.txt

@@ -0,0 +1,130 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rtabmap_rviz_plugins)
+
+# For rviz plugins, look for Qt5 before Qt4
+FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
+IF(NOT Qt5_FOUND)
+    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(NOT Qt5_FOUND)
+
+
+find_package(catkin REQUIRED COMPONENTS
+             roscpp sensor_msgs std_msgs pcl_conversions pluginlib rviz tf rtabmap_conversions rtabmap_msgs)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES rtabmap_rviz_plugins
+  CATKIN_DEPENDS roscpp sensor_msgs std_msgs pcl_conversions pluginlib rviz tf rtabmap_conversions rtabmap_msgs
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## We also use Ogre for rviz plugins
+# this file doesn't exist post-noetic, but pkg_check_modules still works
+IF(EXISTS $ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
+  include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
+ENDIF()
+pkg_check_modules(OGRE OGRE)
+include_directories( ${OGRE_INCLUDE_DIRS} )
+link_directories( ${OGRE_LIBRARY_DIRS} )
+
+SET(Libraries
+  ${catkin_LIBRARIES}
+  ${rviz_DEFAULT_PLUGIN_LIBRARIES}
+)
+
+## RVIZ plugin
+IF(QT4_FOUND)
+  IF(WIN32)
+    qt4_wrap_cpp(MOC_FILES
+      include/${PROJECT_NAME}/MapCloudDisplay.h
+      include/${PROJECT_NAME}/MapGraphDisplay.h
+      include/${PROJECT_NAME}/InfoDisplay.h
+    )
+  ELSE()
+    qt4_wrap_cpp(MOC_FILES
+      include/${PROJECT_NAME}/MapCloudDisplay.h
+      include/${PROJECT_NAME}/MapGraphDisplay.h
+      include/${PROJECT_NAME}/InfoDisplay.h
+      include/${PROJECT_NAME}/OrbitOrientedViewController.h
+    )
+  ENDIF()
+ELSE()
+  IF(WIN32)
+    qt5_wrap_cpp(MOC_FILES
+      include/${PROJECT_NAME}/MapCloudDisplay.h
+      include/${PROJECT_NAME}/MapGraphDisplay.h
+      include/${PROJECT_NAME}/InfoDisplay.h
+    )
+  ELSE()
+    qt5_wrap_cpp(MOC_FILES
+      include/${PROJECT_NAME}/MapCloudDisplay.h
+      include/${PROJECT_NAME}/MapGraphDisplay.h
+      include/${PROJECT_NAME}/InfoDisplay.h
+      include/${PROJECT_NAME}/OrbitOrientedViewController.h
+    )
+  ENDIF()
+ENDIF()
+
+# tf:message_filters, mixing boost and Qt signals
+IF(WIN32)
+  set_property(
+    SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp
+    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
+  )
+ELSE()
+  set_property(
+    SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp src/OrbitOrientedViewController.cpp
+    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
+  )
+ENDIF()
+SET(SRC_FILES 
+   src/MapCloudDisplay.cpp
+   src/MapGraphDisplay.cpp
+   src/InfoDisplay.cpp
+   ${MOC_FILES}
+)
+IF(NOT WIN32)
+  SET(SRC_FILES 
+    ${SRC_FILES} 
+    src/OrbitOrientedViewController.cpp
+  )
+ENDIF(NOT WIN32)
+add_library(rtabmap_rviz_plugins
+   ${SRC_FILES}
+)
+target_link_libraries(rtabmap_rviz_plugins
+  ${Libraries}
+)
+IF(Qt5_FOUND)
+    QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
+ENDIF(Qt5_FOUND)
+
+#############
+## Install ##
+#############
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+)
+
+install(TARGETS 
+   rtabmap_rviz_plugins
+   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+install(FILES
+   rviz_plugins.xml
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/InfoDisplay.h

@@ -0,0 +1,71 @@
+/*
+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 <rtabmap_msgs/Info.h>
+
+#include <rviz/message_filter_display.h>
+#include <rtabmap/core/Transform.h>
+#include <ros/callback_queue.h>
+
+namespace rtabmap_rviz_plugins
+{
+
+class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap_msgs::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::InfoConstPtr& cloud );
+
+private:
+	ros::AsyncSpinner spinner_;
+	ros::CallbackQueue cbqueue_;
+
+	QString info_;
+	int globalCount_;
+	int localCount_;
+	std::map<std::string, float> statistics_;
+	rtabmap::Transform loopTransform_;
+	boost::mutex info_mutex_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif

rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h

@@ -0,0 +1,209 @@
+/*
+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 <deque>
+#include <queue>
+#include <vector>
+
+#include <rtabmap_msgs/MapData.h>
+#include <rtabmap/core/Transform.h>
+
+#include <pluginlib/class_loader.hpp>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <ros/callback_queue.h>
+
+#include <rviz/ogre_helpers/point_cloud.h>
+#include <rviz/message_filter_display.h>
+#include <rviz/default_plugin/point_cloud_transformer.h>
+
+#endif
+
+namespace rviz {
+class IntProperty;
+class BoolProperty;
+class EnumProperty;
+class FloatProperty;
+class PointCloudTransformer;
+typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
+typedef std::vector<std::string> V_string;
+}
+
+using namespace rviz;
+
+namespace rtabmap_rviz_plugins
+{
+
+class PointCloudCommon;
+
+/**
+ * \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 MapCloudDisplay: public rviz::MessageFilterDisplay<rtabmap_msgs::MapData>
+{
+Q_OBJECT
+public:
+	struct CloudInfo
+	  {
+		CloudInfo();
+		~CloudInfo();
+
+		// clear the point cloud, but keep selection handler around
+		void clear();
+
+		Ogre::SceneManager *manager_;
+
+		sensor_msgs::PointCloud2ConstPtr message_;
+		rtabmap::Transform pose_;
+		int id_;
+
+		Ogre::SceneNode *scene_node_;
+		boost::shared_ptr<rviz::PointCloud> cloud_;
+
+		std::vector<rviz::PointCloud::Point> transformed_points_;
+	};
+	typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
+
+	MapCloudDisplay();
+	virtual ~MapCloudDisplay();
+
+	virtual void reset();
+	virtual void update( float wall_dt, float ros_dt );
+
+	rviz::FloatProperty* point_world_size_property_;
+	rviz::FloatProperty* point_pixel_size_property_;
+	rviz::FloatProperty* alpha_property_;
+	rviz::EnumProperty* xyz_transformer_property_;
+	rviz::EnumProperty* color_transformer_property_;
+	rviz::EnumProperty* style_property_;
+	rviz::BoolProperty* cloud_from_scan_;
+	rviz::IntProperty* cloud_decimation_;
+	rviz::FloatProperty* cloud_max_depth_;
+	rviz::FloatProperty* cloud_min_depth_;
+	rviz::FloatProperty* cloud_voxel_size_;
+	rviz::FloatProperty* cloud_filter_floor_height_;
+	rviz::FloatProperty* cloud_filter_ceiling_height_;
+	rviz::FloatProperty* node_filtering_radius_;
+	rviz::FloatProperty* node_filtering_angle_;
+	rviz::StringProperty * download_namespace;
+	rviz::BoolProperty* download_map_;
+	rviz::BoolProperty* download_graph_;
+
+public Q_SLOTS:
+	void causeRetransform();
+
+private Q_SLOTS:
+	void updateStyle();
+	void updateBillboardSize();
+	void updateAlpha();
+	void updateXyzTransformer();
+	void updateColorTransformer();
+	void setXyzTransformerOptions( EnumProperty* prop );
+	void setColorTransformerOptions( EnumProperty* prop );
+	void updateCloudParameters();
+	void downloadNamespaceChanged();
+	void downloadMap();
+	void downloadGraph();
+
+protected:
+	/** @brief Do initialization. Overridden from MessageFilterDisplay. */
+	virtual void onInitialize();
+
+	/** @brief Process a single message.  Overridden from MessageFilterDisplay. */
+	virtual void processMessage( const rtabmap_msgs::MapDataConstPtr& cloud );
+
+private:
+	void downloadMap(bool graphOnly);
+	void processMapData(const rtabmap_msgs::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);
+
+	rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
+	rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
+	void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
+	void retransform();
+
+	void loadTransformers();
+
+	void setPropertiesHidden( const QList<Property*>& props, bool hide );
+	void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask );
+
+private:
+	ros::AsyncSpinner spinner_;
+	ros::CallbackQueue cbqueue_;
+	ros::Publisher republishNodeDataPub_;
+
+	std::map<int, CloudInfoPtr> cloud_infos_;
+
+	std::map<int, CloudInfoPtr> new_cloud_infos_;
+	boost::mutex new_clouds_mutex_;
+
+	std::set<int> nodeDataReceived_;
+	bool fromScan_;
+
+	std::map<int, rtabmap::Transform> current_map_;
+	boost::mutex current_map_mutex_;
+	bool current_map_updated_;
+
+	int lastCloudAdded_;
+
+	struct TransformerInfo
+	{
+		rviz::PointCloudTransformerPtr transformer;
+		QList<Property*> xyz_props;
+		QList<Property*> color_props;
+
+		std::string readable_name;
+		std::string lookup_name;
+	};
+	typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
+
+	boost::recursive_mutex transformers_mutex_;
+	M_TransformerInfo transformers_;
+	bool new_xyz_transformer_;
+	bool new_color_transformer_;
+	bool needs_retransform_;
+
+	pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif

rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapGraphDisplay.h

@@ -0,0 +1,91 @@
+/*
+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_msgs/MapGraph.h>
+
+#include <rviz/message_filter_display.h>
+
+namespace Ogre
+{
+class ManualObject;
+}
+
+namespace rviz
+{
+class ColorProperty;
+class FloatProperty;
+}
+
+using namespace rviz;
+
+namespace rtabmap_rviz_plugins
+{
+
+/**
+ * \class MapGraphDisplay
+ * \brief Displays the graph of rtabmap::MapGraph message
+ */
+class MapGraphDisplay: public MessageFilterDisplay<rtabmap_msgs::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::MapGraph::ConstPtr& msg );
+
+private:
+  void destroyObjects();
+
+  std::vector<Ogre::ManualObject*> manual_objects_;
+
+  ColorProperty* color_neighbor_property_;
+  ColorProperty* color_neighbor_merged_property_;
+  ColorProperty* color_global_property_;
+  ColorProperty* color_local_property_;
+  ColorProperty* color_landmark_property_;
+  ColorProperty* color_user_property_;
+  ColorProperty* color_virtual_property_;
+  FloatProperty* alpha_property_;
+};
+
+} // namespace rtabmap_rviz_plugins
+
+#endif /* MAP_GRAPH_DISPLAY_H */
+

rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/OrbitOrientedViewController.h

@@ -0,0 +1,51 @@
+/*
+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 ORBITORIENTEDVIEWCONTROLLER_H_
+#define ORBITORIENTEDVIEWCONTROLLER_H_
+
+#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
+
+namespace rtabmap_rviz_plugins
+{
+
+class OrbitOrientedViewController: public rviz::OrbitViewController
+{
+Q_OBJECT
+public:
+	OrbitOrientedViewController() {}
+	virtual ~OrbitOrientedViewController() {}
+
+protected:
+  virtual void updateCamera();
+};
+
+}
+
+
+
+#endif /* ORBITORIENTEDVIEWCONTROLLER_H_ */

rtabmap_rviz_plugins/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rtabmap_rviz_plugins</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's rviz plugins.</description>
+  <maintainer email="[email protected]">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>catkin</buildtool_depend>
+
+  <depend>pcl_conversions</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>rtabmap_conversions</depend>
+  <depend>rtabmap_msgs</depend>
+  <depend>rviz</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+
+  <export>
+    <rviz plugin="${prefix}/rviz_plugins.xml"/>
+  </export>
+</package>

rtabmap_rviz_plugins/rviz_plugins.xml

@@ -0,0 +1,33 @@
+<library path="lib/librtabmap_rviz_plugins">
+  <class name="rtabmap_rviz_plugins/MapCloud"
+         type="rtabmap_rviz_plugins::MapCloudDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays graph point clouds from rtabmap_msgs/MapData messages.
+    </description>
+    <message_type>rtabmap_msgs/MapData</message_type>
+  </class>
+  <class name="rtabmap_rviz_plugins/MapGraph"
+         type="rtabmap_rviz_plugins::MapGraphDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays graphs from rtabmap_msgs/MapGraph messages.
+    </description>
+    <message_type>rtabmap_msgs/MapGraph</message_type>
+  </class>
+  <class name="rtabmap_rviz_plugins/Info"
+         type="rtabmap_rviz_plugins::InfoDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays information from rtabmap_msgs/Info messages.
+    </description>
+    <message_type>rtabmap_msgs/Info</message_type>
+  </class>
+  <class name="rtabmap_rviz_plugins/OrbitOriented"
+         type="rtabmap_rviz_plugins::OrbitOrientedViewController"
+         base_class_type="rviz::ViewController">
+    <description>
+      Camera orbit with orientation.
+    </description>
+  </class>
+</library>

rtabmap_rviz_plugins/src/InfoDisplay.cpp

@@ -0,0 +1,132 @@
+/*
+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()
+  : spinner_(1, &cbqueue_),
+    globalCount_(0),
+    localCount_(0)
+{
+	update_nh_.setCallbackQueue( &cbqueue_ );
+}
+
+InfoDisplay::~InfoDisplay()
+{
+	spinner_.stop();
+}
+
+void InfoDisplay::onInitialize()
+{
+	MFDClass::onInitialize();
+
+	this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
+
+	spinner_.start();
+}
+
+void InfoDisplay::processMessage( const rtabmap_msgs::InfoConstPtr& msg )
+{
+	{
+		boost::mutex::scoped_lock lock(info_mutex_);
+		if(msg->loopClosureId)
+		{
+			info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
+			globalCount_ += 1;
+		}
+		else if(msg->proximityDetectionId)
+		{
+			info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
+			localCount_ += 1;
+		}
+		else
+		{
+			info_ = "";
+		}
+		loopTransform_ = rtabmap_conversions::transformFromGeometryMsg(msg->loopClosureTransform);
+
+		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 )
+{
+	{
+		boost::mutex::scoped_lock lock(info_mutex_);
+		this->setStatusStd(rviz::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString());
+		if(loopTransform_.isNull())
+		{
+			this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
+			this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
+		}
+		else
+		{
+			float x,y,z, roll,pitch,yaw;
+			loopTransform_.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
+			this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
+		}
+		this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
+		this->setStatusStd(rviz::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::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
+		}
+	}
+}
+
+void InfoDisplay::reset()
+{
+	MFDClass::reset();
+	{
+		boost::mutex::scoped_lock 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::Display )

rtabmap_rviz_plugins/src/MapCloudDisplay.cpp

@@ -0,0 +1,997 @@
+/*
+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 <ros/time.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
+#include <rviz/ogre_helpers/point_cloud.h>
+#include <rviz/validate_floats.h>
+#include <rviz/properties/int_property.h>
+#include "rviz/properties/bool_property.h"
+#include "rviz/properties/enum_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/vector_property.h"
+
+#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/GetMap.h>
+#include <std_msgs/Int32MultiArray.h>
+
+
+namespace rtabmap_rviz_plugins
+{
+
+
+MapCloudDisplay::CloudInfo::CloudInfo() :
+		manager_(0),
+		pose_(rtabmap::Transform::getIdentity()),
+		id_(0),
+		scene_node_(0)
+{}
+
+MapCloudDisplay::CloudInfo::~CloudInfo()
+{
+	clear();
+}
+
+void MapCloudDisplay::CloudInfo::clear()
+{
+	if ( scene_node_ )
+	{
+		manager_->destroySceneNode( scene_node_ );
+		scene_node_=0;
+	}
+}
+
+MapCloudDisplay::MapCloudDisplay()
+  : spinner_(1, &cbqueue_),
+	lastCloudAdded_(-1),
+    new_xyz_transformer_(false),
+    new_color_transformer_(false),
+    needs_retransform_(false),
+    transformer_class_loader_(NULL),
+	current_map_updated_(false)
+{
+	//QIcon icon;
+	//this->setIcon(icon);
+
+	style_property_ = new rviz::EnumProperty( "Style", "Flat Squares",
+										"Rendering mode to use, in order of computational complexity.",
+										this, SLOT( updateStyle() ), this );
+	style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS );
+	style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES );
+	style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES );
+	style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES );
+	style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES );
+
+	point_world_size_property_ = new rviz::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::FloatProperty( "Size (Pixels)", 3,
+												  "Point size in pixels.",
+												  this, SLOT( updateBillboardSize() ), this );
+	point_pixel_size_property_->setMin( 1 );
+
+	alpha_property_ = new rviz::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::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( EnumProperty* )),
+			 this, SLOT( setXyzTransformerOptions( EnumProperty* )));
+
+	color_transformer_property_ = new rviz::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( EnumProperty* )),
+			 this, SLOT( setColorTransformerOptions( EnumProperty* )));
+
+	cloud_from_scan_ = new rviz::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::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::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::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::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::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::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::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::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::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
+
+	download_map_ = new rviz::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::BoolProperty( "Download graph", false,
+											 "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
+											 this, SLOT( downloadGraph() ), this );
+
+	downloadNamespaceChanged();
+
+	// PointCloudCommon sets up a callback queue with a thread for each
+	// instance.  Use that for processing incoming messages.
+	update_nh_.setCallbackQueue( &cbqueue_ );
+}
+
+MapCloudDisplay::~MapCloudDisplay()
+{
+	if ( transformer_class_loader_ )
+	{
+		delete transformer_class_loader_;
+	}
+
+	spinner_.stop();
+}
+
+void MapCloudDisplay::loadTransformers()
+{
+	std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
+	std::vector<std::string>::iterator ci;
+
+	for( ci = classes.begin(); ci != classes.end(); ci++ )
+	{
+		const std::string& lookup_name = *ci;
+		std::string name = transformer_class_loader_->getName( lookup_name );
+
+		if( transformers_.count( name ) > 0 )
+		{
+			ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
+			continue;
+		}
+
+		rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( 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::PointCloudTransformer::Support_XYZ, info.xyz_props );
+		setPropertiesHidden( info.xyz_props, true );
+
+		info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props );
+		setPropertiesHidden( info.color_props, true );
+
+		transformers_[ name ] = info;
+	}
+}
+
+void MapCloudDisplay::onInitialize()
+{
+	MFDClass::onInitialize();
+
+	transformer_class_loader_ = new pluginlib::ClassLoader<rviz::PointCloudTransformer>( "rviz", "rviz::PointCloudTransformer" );
+	loadTransformers();
+
+	updateStyle();
+	updateBillboardSize();
+	updateAlpha();
+
+	spinner_.start();
+}
+
+void MapCloudDisplay::processMessage( const rtabmap_msgs::MapDataConstPtr& msg )
+{
+	processMapData(*msg);
+
+	this->emitTimeSignal(msg->header.stamp);
+}
+
+void MapCloudDisplay::processMapData(const rtabmap_msgs::MapData& map)
+{
+	std::map<int, rtabmap::Transform> poses;
+	for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(map.graph.posesId[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::nodeDataFromROS(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::PointCloud2::Ptr cloudMsg(new sensor_msgs::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
+						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))
+				{
+					boost::mutex::scoped_lock 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);
+	}
+
+	{
+		boost::mutex::scoped_lock lock(current_map_mutex_);
+		current_map_ = poses;
+		current_map_updated_ = true;
+		nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
+	}
+}
+
+void MapCloudDisplay::setPropertiesHidden( const QList<Property*>& props, bool hide )
+{
+	for( int i = 0; i < props.size(); i++ )
+	{
+		props[ i ]->setHidden( hide );
+	}
+}
+
+void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& 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;
+	bool has_intensity_transformer = false;
+	M_TransformerInfo::iterator trans_it = transformers_.begin();
+	M_TransformerInfo::iterator trans_end = transformers_.end();
+	for(;trans_it != trans_end; ++trans_it)
+	{
+		const std::string& name = trans_it->first;
+		const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer;
+		uint32_t mask = trans->supports(cloud);
+		if (mask & rviz::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::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;
+			}
+			else if (name == "Intensity")
+			{
+				has_intensity_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 if (has_intensity_transformer)
+			{
+				color_transformer_property_->setStringStd( "Intensity" );
+			}
+			else
+			{
+				color_transformer_property_->setStringStd( valid_color.rbegin()->second );
+			}
+		}
+	}
+}
+
+void MapCloudDisplay::updateAlpha()
+{
+	for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
+	{
+		it->second->cloud_->setAlpha( alpha_property_->getFloat() );
+	}
+}
+
+void MapCloudDisplay::updateStyle()
+{
+	rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+	if( mode == rviz::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( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
+	{
+		it->second->cloud_->setRenderMode( mode );
+	}
+	updateBillboardSize();
+}
+
+void MapCloudDisplay::updateBillboardSize()
+{
+	rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+	float size;
+	if( mode == rviz::PointCloud::RM_POINTS )
+	{
+		size = point_pixel_size_property_->getFloat();
+	}
+	else
+	{
+		size = point_world_size_property_->getFloat();
+	}
+	 for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
+	{
+		it->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)
+{
+	rtabmap_msgs::GetMap getMapSrv;
+	getMapSrv.request.global = false;
+	getMapSrv.request.optimized = true;
+	getMapSrv.request.graphOnly = graphOnly;
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string srvName = update_nh_.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
+	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);
+	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+	messageBox->show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
+	if(!ros::service::call(srvName, getMapSrv))
+	{
+		ROS_ERROR("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());
+		messageBox->setText(tr("MapCloudDisplay: Cannot call \"%1\" service. "
+				  "Tip: if rtabmap node is not in \"%2\" namespace, you can "
+				  "change the \"Download namespace\" option.").
+				  arg(srvName.c_str()).arg(rtabmapNs.c_str()));
+	}
+	else if(graphOnly)
+	{
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
+		QApplication::processEvents();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+	else
+	{
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+		QApplication::processEvents();
+		this->reset();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+}
+
+void MapCloudDisplay::downloadNamespaceChanged()
+{
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string topicName = update_nh_.resolveName(uFormat("%s/republish_node_data", rtabmapNs.c_str()));
+	republishNodeDataPub_ = update_nh_.advertise<std_msgs::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 wall_dt, float ros_dt )
+{
+	rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+
+	int lastCloudAdded = -1;
+
+	if (needs_retransform_)
+	{
+		retransform();
+		needs_retransform_ = false;
+	}
+
+	{
+		boost::mutex::scoped_lock lock(new_clouds_mutex_);
+		if( !new_cloud_infos_.empty() )
+		{
+			float size;
+			if( mode == rviz::PointCloud::RM_POINTS ) {
+				size = point_pixel_size_property_->getFloat();
+			} else {
+				size = point_world_size_property_->getFloat();
+			}
+
+			std::map<int, CloudInfoPtr>::iterator it = new_cloud_infos_.begin();
+			std::map<int, CloudInfoPtr>::iterator end = new_cloud_infos_.end();
+			for (; it != end; ++it)
+			{
+				CloudInfoPtr cloud_info = it->second;
+
+				cloud_info->cloud_.reset( new rviz::PointCloud() );
+				cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
+				cloud_info->cloud_->setRenderMode( mode );
+				cloud_info->cloud_->setAlpha( alpha_property_->getFloat() );
+				cloud_info->cloud_->setDimensions( size, size, size );
+				cloud_info->cloud_->setAutoSize(false);
+
+				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();
+		}
+	}
+
+	{
+		boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
+
+		if( lock.owns_lock() )
+		{
+			if( new_xyz_transformer_ || new_color_transformer_ )
+			{
+				M_TransformerInfo::iterator it = transformers_.begin();
+				M_TransformerInfo::iterator end = transformers_.end();
+				for (; it != end; ++it)
+				{
+					const std::string& name = it->first;
+					TransformerInfo& info = it->second;
+
+					setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
+					setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
+				}
+			}
+		}
+
+		new_xyz_transformer_ = false;
+		new_color_transformer_ = false;
+	}
+
+	int totalPoints = 0;
+	int totalNodesShown = 0;
+	{
+		// update poses
+		boost::mutex::scoped_lock 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, 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.extractQuaternion();
+						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()->frameHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
+					{
+						ROS_ERROR("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())
+			{
+				std_msgs::Int32MultiArray msg;
+				msg.data = missingNodes;
+				republishNodeDataPub_.publish(msg);
+			}
+		}
+		current_map_updated_ = false;
+	}
+	if(lastCloudAdded>0)
+	{
+		lastCloudAdded_ = lastCloudAdded;
+	}
+
+	this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
+	this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
+}
+
+void MapCloudDisplay::reset()
+{
+	lastCloudAdded_ = -1;
+	{
+		boost::mutex::scoped_lock lock(new_clouds_mutex_);
+		cloud_infos_.clear();
+		new_cloud_infos_.clear();
+	}
+	{
+		boost::mutex::scoped_lock lock(current_map_mutex_);
+		current_map_.clear();
+		current_map_updated_ = false;
+		nodeDataReceived_.clear();
+	}
+	MFDClass::reset();
+}
+
+void MapCloudDisplay::updateXyzTransformer()
+{
+	boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+	if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
+	{
+		return;
+	}
+	new_xyz_transformer_ = true;
+	causeRetransform();
+}
+
+void MapCloudDisplay::updateColorTransformer()
+{
+	boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+	if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
+	{
+		return;
+	}
+	new_color_transformer_ = true;
+	causeRetransform();
+}
+
+void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop )
+{
+	fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ );
+}
+
+void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop )
+{
+	fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color );
+}
+
+void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask )
+{
+	prop->clearOptions();
+
+	if (cloud_infos_.empty())
+	{
+		return;
+	}
+
+	boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
+
+	const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_;
+
+	M_TransformerInfo::iterator it = transformers_.begin();
+	M_TransformerInfo::iterator end = transformers_.end();
+	for (; it != end; ++it)
+	{
+		const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+		if ((trans->supports(msg) & mask) == mask)
+		{
+			prop->addOption( QString::fromStdString( it->first ));
+		}
+	}
+}
+
+rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
+{
+	boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
+	M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
+	if( it != transformers_.end() )
+	{
+		const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+		if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ )
+		{
+			return trans;
+		}
+	}
+
+	return rviz::PointCloudTransformerPtr();
+}
+
+rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
+{
+	boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+	M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
+	if( it != transformers_.end() )
+	{
+		const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+		if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color )
+		{
+			return trans;
+		}
+	}
+
+	return rviz::PointCloudTransformerPtr();
+}
+
+
+void MapCloudDisplay::retransform()
+{
+	boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
+
+	for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
+	{
+		const CloudInfoPtr& cloud_info = it->second;
+		transformCloud(cloud_info, false);
+		cloud_info->cloud_->clear();
+		cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
+	}
+}
+
+bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
+{
+	rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
+	cloud_points.clear();
+
+	size_t size = cloud_info->message_->width * cloud_info->message_->height;
+	rviz::PointCloud::Point default_pt;
+	default_pt.color = Ogre::ColourValue(1, 1, 1);
+	default_pt.position = Ogre::Vector3::ZERO;
+	cloud_points.resize(size, default_pt);
+
+	{
+		boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
+		if( update_transformers )
+		{
+			updateTransformers( cloud_info->message_ );
+		}
+		rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
+		rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
+
+		if (!xyz_trans)
+		{
+			std::stringstream ss;
+			ss << "No position transformer available for cloud";
+			this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
+			return false;
+		}
+
+		if (!color_trans)
+		{
+			std::stringstream ss;
+			ss << "No color transformer available for cloud";
+			this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
+			return false;
+		}
+
+		xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
+		color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
+	}
+
+	for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
+	{
+		if (!rviz::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::Display )

rtabmap_rviz_plugins/src/MapGraphDisplay.cpp

@@ -0,0 +1,186 @@
+/*
+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 <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreManualObject.h>
+#include <OgreBillboardSet.h>
+#include <OgreMatrix4.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
+#include "rviz/properties/color_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/int_property.h"
+
+#include <rtabmap/core/Link.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+namespace rtabmap_rviz_plugins
+{
+
+MapGraphDisplay::MapGraphDisplay()
+{
+	color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue,
+	                                       "Color to draw neighbor links.", this );
+	color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0),
+	                                       "Color to draw merged neighbor links.", this );
+	color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red,
+	                                       "Color to draw global loop closure links.", this );
+	color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow,
+	                                       "Color to draw local loop closure links.", this );
+	color_landmark_property_ = new rviz::ColorProperty( "Landmark", Qt::darkGreen,
+	                                       "Color to draw landmark links.", this );
+	color_user_property_ = new rviz::ColorProperty( "User", Qt::red,
+	                                       "Color to draw user links.", this );
+	color_virtual_property_ = new rviz::ColorProperty( "Virtual", Qt::magenta,
+	                                       "Color to draw virtual links.", this );
+
+	alpha_property_ = new rviz::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::MapGraph::ConstPtr& msg )
+{
+	if(!(msg->poses.size() == msg->posesId.size()))
+	{
+		ROS_ERROR("rtabmap_msgs::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 ))
+	{
+		ROS_DEBUG( "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::Display )

rtabmap_rviz_plugins/src/OrbitOrientedViewController.cpp

@@ -0,0 +1,76 @@
+/*
+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/OrbitOrientedViewController.h"
+
+#include <OgreCamera.h>
+#include <OgreQuaternion.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#include <OgreViewport.h>
+
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/vector_property.h"
+#include "rviz/ogre_helpers/shape.h"
+
+namespace rtabmap_rviz_plugins
+{
+
+void OrbitOrientedViewController::updateCamera()
+{
+	float distance = distance_property_->getFloat();
+	float yaw = yaw_property_->getFloat();
+	float pitch = pitch_property_->getFloat();
+
+	Ogre::Matrix3 rot;
+	reference_orientation_.ToRotationMatrix(rot);
+	Ogre::Radian rollTarget, pitchTarget, yawTarget;
+	rot.ToEulerAnglesXYZ(yawTarget, pitchTarget, rollTarget);
+
+	yaw += rollTarget.valueRadians();
+	pitch += pitchTarget.valueRadians();
+
+	Ogre::Vector3 focal_point = focal_point_property_->getVector();
+
+	float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
+	float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
+	float z = distance *              sin( pitch ) + focal_point.z;
+
+	Ogre::Vector3 pos( x, y, z );
+
+	camera_->setPosition(pos);
+	camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
+	camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));
+
+	focal_shape_->setPosition( focal_point );
+}
+
+}
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS( rtabmap_rviz_plugins::OrbitOrientedViewController, rviz::ViewController )