i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1555 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-13 21:07:25 +00:00
parent c5cd0b7517
commit d6aeaa5807
7 changed files with 142 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/InfoDisplay.cpp
+78
View File
@@ -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 )