i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +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.");
}
}
}
rtabmap_odom/src/nodelets/icp_odometry.cpp
+7
View File
@@ -157,6 +157,11 @@ private:
cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
getName().c_str(),
scan_sub_.getTopic().c_str(),
cloud_sub_.getTopic().c_str()), true);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -329,6 +334,7 @@ private:
scan_sub_.shutdown();
return;
}
scanReceived_ = true;
if(this->isPaused())
{
@@ -570,6 +576,7 @@ private:
cloud_sub_.shutdown();
return;
}
cloudReceived_ = true;
if(this->isPaused())
{
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+3 -8
View File
@@ -161,6 +161,7 @@ private:
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -356,6 +357,7 @@ private:
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
subscribedTopic = rgb_nh.resolveName("image");
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
@@ -364,7 +366,7 @@ private:
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -546,7 +548,6 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -575,7 +576,6 @@ private:
void callbackRGBD(
const rtabmap_msgs::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -591,7 +591,6 @@ private:
void callbackRGBDX(
const rtabmap_msgs::RGBDImagesConstPtr& images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -616,7 +615,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -636,7 +634,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -659,7 +656,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -685,7 +681,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image4,
const rtabmap_msgs::RGBDImageConstPtr& image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
+1 -2
View File
@@ -202,7 +202,7 @@ private:
info_sub_.getTopic().c_str(),
scan_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -243,7 +243,6 @@ private:
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
callbackCalled();
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+3 -8
View File
@@ -111,6 +111,7 @@ private:
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -271,7 +272,7 @@ private:
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
subscribedTopic = left_nh.resolveName("image_rect");
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
@@ -282,7 +283,7 @@ private:
cameraInfoRight_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -578,7 +579,6 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -609,7 +609,6 @@ private:
void callbackRGBD(
const rtabmap_msgs::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -627,7 +626,6 @@ private:
void callbackRGBDX(
const rtabmap_msgs::RGBDImagesConstPtr& images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -654,7 +652,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -677,7 +674,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -704,7 +700,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);