i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
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_;
};