RVIZ: added rtabmap/InfoDisplay to show loop closure information from rtabmap::Info msgs

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1555 f169173b-cf89-36c8-b27e-44dbe73f0c83

src/rviz/InfoDisplay.cpp

@@ -0,0 +1,78 @@
+
+#include "InfoDisplay.h"
+
+namespace rtabmap
+{
+
+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, "Global", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0");
+
+	spinner_.start();
+}
+
+void InfoDisplay::processMessage( const rtabmap::InfoConstPtr& msg )
+{
+	{
+		boost::mutex::scoped_lock lock(info_mutex_);
+		if(msg->loopClosureId)
+		{
+			info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId);
+			globalCount_ += 1;
+		}
+		else if(msg->localLoopClosureId)
+		{
+			info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId);
+			localCount_ += 1;
+		}
+		else
+		{
+			info_ = "";
+		}
+	}
+
+	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());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString());
+	}
+}
+
+void InfoDisplay::reset()
+{
+	MFDClass::reset();
+	{
+		boost::mutex::scoped_lock lock(info_mutex_);
+		info_.clear();
+		globalCount_ = 0;
+		localCount_ = 0;
+	}
+}
+
+} // namespace rtabmap
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( rtabmap::InfoDisplay, rviz::Display )

src/rviz/InfoDisplay.h

@@ -0,0 +1,41 @@
+
+#ifndef INFO_DISPLAY_H
+#define INFO_DISPLAY_H
+
+#include <rtabmap/Info.h>
+
+#include <rviz/message_filter_display.h>
+
+namespace rtabmap
+{
+
+class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap::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::InfoConstPtr& cloud );
+
+private:
+	ros::AsyncSpinner spinner_;
+	ros::CallbackQueue cbqueue_;
+
+	QString info_;
+	int globalCount_;
+	int localCount_;
+	boost::mutex info_mutex_;
+};
+
+} // namespace rtabmap
+
+#endif

src/rviz/MapCloudDisplay.cpp

@@ -795,7 +795,7 @@ bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update
 	return true;
 }
 
-} // namespace rviz
+} // namespace rtabmap
 
 #include <pluginlib/class_list_macros.h>
 PLUGINLIB_EXPORT_CLASS( rtabmap::MapCloudDisplay, rviz::Display )

src/rviz/MapCloudDisplay.h

@@ -34,8 +34,8 @@ namespace rtabmap
 class PointCloudCommon;
 
 /**
- * \class PointCloudDisplay
- * \brief Displays a point cloud of type sensor_msgs::PointCloud
+ * \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