i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
updated azimut3.rviz with OrbitOriented view

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1556 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-13 21:27:30 +00:00
parent d6aeaa5807
commit 6d3a2a7d3a
3 changed files with 38 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/config/azimut3.rviz
+20 -67
View File
@@ -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
Min Value: -3.31707
Max Value: 0.167568
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
Distance: 14.6712
Class: rtabmap/OrbitOriented
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
Y: -1.75501
Z: 0.714409
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 1.0448
Pitch: 0.8148
Target Frame: base_link
Value: Orbit (rviz)
Yaw: 0.872834
Value: OrbitOriented (rtabmap)
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
+16
View File
@@ -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
+2
View File
@@ -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_;
};