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

CMakeLists.txt

@@ -105,12 +105,13 @@ SET(Libraries
 ## RVIZ plugin
 qt4_wrap_cpp(MOC_FILES
   src/rviz/MapCloudDisplay.h
+  src/rviz/InfoDisplay.h
   src/rviz/OrbitOrientedViewController.h
 )
 
 # tf:message_filters, mixing boost and Qt signals
 set_property(
-   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
+   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
    )
    
@@ -121,6 +122,7 @@ add_library(rtabmap_ros
    src/nodelets/point_cloud_xyzrgb.cpp 
    src/MsgConversion.cpp
    src/rviz/MapCloudDisplay.cpp
+   src/rviz/InfoDisplay.cpp
    src/rviz/OrbitOrientedViewController.cpp
    ${MOC_FILES}
 )

launch/config/rgbd.rviz

@@ -6,7 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /MapCloud1/Status1
+        - /Info1
+        - /Info1/Status1
       Splitter Ratio: 0.411058
     Tree Height: 422
   - Class: rviz/Selection
@@ -26,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: MapCloud
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -84,35 +85,14 @@ Visualization Manager:
         All Enabled: false
         base_link:
           Value: true
-        camera_depth_frame:
-          Value: true
-        camera_depth_optical_frame:
-          Value: true
         camera_link:
           Value: true
-        camera_rgb_frame:
-          Value: true
-        camera_rgb_optical_frame:
-          Value: true
-        map:
-          Value: true
-        odom:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          odom:
-            base_link:
-              camera_link:
-                camera_depth_frame:
-                  camera_depth_optical_frame:
-                    {}
-                camera_rgb_frame:
-                  camera_rgb_optical_frame:
         {}
       Update Interval: 0
       Value: true
@@ -196,6 +176,11 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rtabmap/Info
+      Enabled: true
+      Name: Info
+      Topic: /rtabmap/info
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -251,5 +236,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1273
-  X: 100
+  X: 93
   Y: 115

plugin_description.xml

@@ -6,6 +6,13 @@
       Displays graph point clouds from rtabmap/MapData messages.
     </description>
   </class>
+  <class name="rtabmap/Info"
+         type="rtabmap::InfoDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays information from rtabmap/Info messages.
+    </description>
+  </class>
   <class name="rtabmap/OrbitOriented"
          type="rtabmap::OrbitOrientedViewController"
          base_class_type="rviz::ViewController">

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