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

created ros2 branch

Browse Source
This commit is contained in:
matlabbe
2020-01-30 17:18:48 -05:00
parent d80a66a724
commit e6ea46f5f0
86 changed files with 9079 additions and 9079 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/InfoDisplay.cpp
+24 -29
View File
@@ -32,50 +32,45 @@ namespace rtabmap_ros
{
InfoDisplay::InfoDisplay()
: spinner_(1, &cbqueue_),
globalCount_(0),
: 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, "Position (XYZ)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
spinner_.start();
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Info", "");
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", "");
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", "0");
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", "0");
}
void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
void InfoDisplay::processMessage( const rtabmap_ros::msg::Info::ConstSharedPtr msg )
{
{
boost::mutex::scoped_lock lock(info_mutex_);
if(msg->loopClosureId)
std::unique_lock<std::mutex> lock(info_mutex_);
if(msg->loop_closure_id)
{
info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
info_ = QString("%1->%2").arg(msg->ref_id).arg(msg->loop_closure_id);
globalCount_ += 1;
}
else if(msg->proximityDetectionId)
else if(msg->proximity_detection_id)
{
info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
info_ = QString("%1->%2 [Proximity]").arg(msg->ref_id).arg(msg->proximity_detection_id);
localCount_ += 1;
}
else
{
info_ = "";
}
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform);
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loop_closure_transform);
rtabmap::Statistics stat;
rtabmap_ros::infoFromROS(*msg, stat);
@@ -89,26 +84,26 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
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());
std::unique_lock<std::mutex> lock(info_mutex_);
this->setStatusStd(rviz_common::properties::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)", "");
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
this->setStatusStd(rviz_common::properties::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_common::properties::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
}
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
{
this->setStatus(rviz::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
this->setStatus(rviz_common::properties::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
}
}
}
@@ -117,7 +112,7 @@ void InfoDisplay::reset()
{
MFDClass::reset();
{
boost::mutex::scoped_lock lock(info_mutex_);
std::unique_lock<std::mutex> lock(info_mutex_);
info_.clear();
globalCount_ = 0;
localCount_ = 0;
@@ -128,4 +123,4 @@ void InfoDisplay::reset()
} // namespace rtabmap_ros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz_common::Display )