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

Added sync diagnostic (#1026)

Browse Source 
* Added sync diagnotic

* Removed composite task to avoid empty msg, added odom status task
This commit is contained in:
matlabbe
2023-08-27 12:30:53 -07:00
committed by GitHub
parent cd52f7664c
commit 4a3863cdc7
28 changed files with 264 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+40 -25
View File
@@ -57,9 +57,8 @@ using namespace rtabmap;
namespace rtabmap_odom {
OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
rtabmap_sync::SyncDiagnostic(0.5),
odometry_(0),
warningThread_(0),
callbackCalled_(false),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
@@ -91,13 +90,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
OdometryROS::~OdometryROS()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}
delete odometry_;
}
@@ -378,29 +370,20 @@ void OdometryROS::onInit()
onOdomInit();
}
void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
}
void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
getName().c_str(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
}
}
subscribedTopicsMsg.c_str()),
tasks);
}
rtabmap::Transform OdometryROS::velocityGuess() const
@@ -953,6 +936,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
{
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
}
previousStamp_ = header.stamp.toSec();
}
}
@@ -1038,5 +1032,26 @@ bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
return true;
}
OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
{}
void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
lost_ = isLost;
}
void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Lost!");
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Tracking.");
}
}
}