From 4bb227d5238d28446ae13eeb1e12fdb384031f67 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Aug 2023 12:29:25 -0700 Subject: [PATCH] Removed composite task to avoid empty msg, added odom status task --- .../include/rtabmap_odom/OdometryROS.h | 15 ++++++++- rtabmap_odom/src/OdometryROS.cpp | 33 ++++++++++++++++--- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 4 ++- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 5 +-- .../include/rtabmap_sync/SyncDiagnostic.h | 22 ++++++------- 5 files changed, 59 insertions(+), 20 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index d7cbe72cf..b26f734de 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include #include @@ -78,7 +80,7 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic bool isPaused() const {return paused_;} protected: - void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync); + void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = ""); virtual void flushCallbacks() = 0; tf::TransformListener & tfListener() {return tfListener_;} @@ -151,6 +153,17 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic std::pair bufferedData_; rtabmap_util::ULogToRosout ulogToRosout_; + + class OdomStatusTask : public diagnostic_updater::DiagnosticTask + { + public: + OdomStatusTask(); + void setStatus(bool isLost); + void run(diagnostic_updater::DiagnosticStatusWrapper &stat); + private: + bool lost_; + }; + OdomStatusTask statusDiagnostic_; }; } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 68fe607c3..db05d3913 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -370,17 +370,20 @@ void OdometryROS::onInit() onOdomInit(); } -void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync) +void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic) { NODELET_INFO("%s", subscribedTopicsMsg.c_str()); - initDiagnostic("", + std::vector 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 @@ -934,10 +937,11 @@ 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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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, + 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); @@ -1028,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."); + } +} } diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 1eebfe501..1cb190897 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -161,6 +161,7 @@ class RGBDOdometry : public OdometryROS 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 @@ class RGBDOdometry : public OdometryROS 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 @@ class RGBDOdometry : public OdometryROS image_depth_sub_.getTopic().c_str(), info_sub_.getTopic().c_str()); } - initDiagnosticMsg(subscribedTopicsMsg, approxSync); + initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic); } virtual void updateParameters(ParametersMap & parameters) diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index c279b1fe9..6d65f2f91 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -111,6 +111,7 @@ class StereoOdometry : public OdometryROS 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 @@ class StereoOdometry : public OdometryROS 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 @@ class StereoOdometry : public OdometryROS cameraInfoRight_.getTopic().c_str()); } - initDiagnosticMsg(subscribedTopicsMsg, approxSync); + initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic); } virtual void updateParameters(ParametersMap & parameters) diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 579ee053b..31aba53de 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -11,7 +11,6 @@ namespace rtabmap_sync { class SyncDiagnostic { public: SyncDiagnostic(double tolerance = 0.1) : - compositeTask_("Sync Status"), frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)), lastCallbackCalledStamp_(ros::Time::now().toSec()-1), targetFrequency_(0.0) @@ -19,21 +18,24 @@ class SyncDiagnostic { protected: void initDiagnostic( - const std::string & imageTopic, - const std::string & topicsNotReceivedWarningMsg) + const std::string & topic, + const std::string & topicsNotReceivedWarningMsg, + std::vector otherTasks = std::vector()) { topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg; - std::list strList = uSplit(imageTopic, '/'); + std::list strList = uSplit(topic, '/'); for(int i=0; i<2 && strList.size()>1; ++i) { // Assuming format is /back_camera/left/image, we want "back_camera" strList.pop_back(); } - compositeTask_.addTask(&frequencyStatus_); - compositeTask_.addTask(&timeStampStatus_); + diagnosticUpdater_.add(frequencyStatus_); + for(size_t i=0; i0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_)) { @@ -52,7 +53,6 @@ class SyncDiagnostic { targetFrequency_ = targetFrequency; } lastCallbackCalledStamp_ = stamp.toSec(); - diagnosticUpdater_.update(); } private: @@ -69,9 +69,7 @@ class SyncDiagnostic { private: std::string topicsNotReceivedWarningMsg_; diagnostic_updater::Updater diagnosticUpdater_; - diagnostic_updater::CompositeDiagnosticTask compositeTask_; diagnostic_updater::FrequencyStatus frequencyStatus_; - diagnostic_updater::SlowTimeStampStatus timeStampStatus_; ros::Timer diagnosticTimer_; double lastCallbackCalledStamp_; double targetFrequency_; @@ -80,4 +78,4 @@ class SyncDiagnostic { } -#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */ \ No newline at end of file +#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */