rviz InfoDisplay plugin: added position and orientation properties for the loop closure transform

updated azimut3.rviz with OrbitOriented view

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

launch/config/azimut3.rviz

@@ -6,6 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
+        - /Info1
+        - /Info1/Status1
       Splitter Ratio: 0.5
     Tree Height: 527
   - Class: rviz/Selection
@@ -19,6 +21,7 @@ Panels:
   - Class: rviz/Views
     Expanded:
       - /Current View1
+      - /Current View1/Focal Point1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
@@ -153,8 +156,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: -0.64594
+        Max Value: 0.167568
-        Min Value: -3.31707
+        Min Value: -4.37384
         Value: true
       Axis: X
       Channel Name: intensity
@@ -197,61 +200,6 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base_footprint:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_laser_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelLB_linkWheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelLB_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelLF_linkWheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelLF_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelRB_linkWheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelRB_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelRF_linkWheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheelRF_wheel_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -301,6 +249,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
@@ -320,23 +273,23 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
+      Class: rtabmap/OrbitOriented
-      Distance: 14.6712
+      Distance: 8.51031
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -1.17324
+        X: 0
-        Y: -1.75501
+        Y: 0
-        Z: 0.714409
+        Z: 0
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.0448
+      Pitch: 0.8148
       Target Frame: base_link
-      Value: Orbit (rviz)
+      Value: OrbitOriented (rtabmap)
-      Yaw: 0.872834
+      Yaw: 3.1456
     Saved: ~
 Window Geometry:
   Displays:
@@ -346,7 +299,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000013600000377fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000000000029e000000dd00fffffffb0000000a0049006d00610067006501000002a4000000d30000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004dd0000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000013600000377fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000000000029e000000dd00fffffffb0000000a0049006d00610067006501000002a4000000d30000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004dd0000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -356,5 +309,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1561
-  X: 65
+  X: 58
   Y: 98

src/rviz/InfoDisplay.cpp

@@ -1,5 +1,6 @@
 
 #include "InfoDisplay.h"
+#include "rtabmap/MsgConversion.h"
 
 namespace rtabmap
 {
@@ -22,6 +23,8 @@ 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, "Global", "0");
 	this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0");
 
@@ -46,6 +49,7 @@ void InfoDisplay::processMessage( const rtabmap::InfoConstPtr& msg )
 		{
 			info_ = "";
 		}
+		loopTransform_ = transformFromGeometryMsg(msg->loopClosureTransform);
 	}
 
 	this->emitTimeSignal(msg->header.stamp);
@@ -56,6 +60,18 @@ 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, "Global", tr("%1").arg(globalCount_).toStdString());
 		this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString());
 	}

src/rviz/InfoDisplay.h

@@ -5,6 +5,7 @@
 #include <rtabmap/Info.h>
 
 #include <rviz/message_filter_display.h>
+#include <rtabmap/core/Transform.h>
 
 namespace rtabmap
 {
@@ -33,6 +34,7 @@ private:
 	QString info_;
 	int globalCount_;
 	int localCount_;
+	Transform loopTransform_;
 	boost::mutex info_mutex_;
 };