Added sync diagnostic (#1026)

* Added sync diagnotic * Removed composite task to avoid empty msg, added odom status task
introlab · Aug 27, 2023 · 4a3863c · 4a3863c
1 parent cd52f76
commit 4a3863c
Show file tree

Hide file tree

Showing 28 changed files with 268 additions and 289 deletions.
diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt
@@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
              cv_bridge image_geometry laser_geometry message_filters 
              nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
              sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+             rtabmap_sync
 )
 
 catkin_package(
@@ -13,6 +14,7 @@ catkin_package(
   CATKIN_DEPENDS cv_bridge image_geometry laser_geometry message_filters 
              nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
              sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+             rtabmap_sync
 )
 
 ###########

diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -38,21 +38,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Header.h>
 #include <sensor_msgs/Imu.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+
 #include <rtabmap_msgs/ResetPose.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 
 #include <boost/thread.hpp>
 
 #include "rtabmap_util/ULogToRosout.h"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap {
 class Odometry;
 }
 
 namespace rtabmap_odom {
 
-class OdometryROS : public nodelet::Nodelet
+class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
 {
 
 public:
@@ -77,8 +80,7 @@ class OdometryROS : public nodelet::Nodelet
 	bool isPaused() const {return paused_;}
 
 protected:
-	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
-	void callbackCalled() {callbackCalled_ = true;}
+	void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
 
 	virtual void flushCallbacks() = 0;
 	tf::TransformListener & tfListener() {return tfListener_;}
@@ -88,7 +90,6 @@ class OdometryROS : public nodelet::Nodelet
 	virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
 
 private:
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
 	virtual void onInit();
 	virtual void onOdomInit() = 0;
 	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
@@ -98,8 +99,6 @@ class OdometryROS : public nodelet::Nodelet
 
 private:
 	rtabmap::Odometry * odometry_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;
@@ -154,6 +153,17 @@ class OdometryROS : public nodelet::Nodelet
 	std::pair<rtabmap::SensorData, std_msgs::Header > 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/package.xml b/rtabmap_odom/package.xml
@@ -25,6 +25,7 @@
   <depend>rtabmap_conversions</depend>
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>
+  <depend>rtabmap_sync</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />

diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
@@ -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.");
+	}
+}
 
 }
diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp
@@ -157,6 +157,11 @@ class ICPOdometry : public OdometryROS
 		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 @@ class ICPOdometry : public OdometryROS
 			scan_sub_.shutdown();
 			return;
 		}
+
 		scanReceived_ = true;
 		if(this->isPaused())
 		{
@@ -570,6 +576,7 @@ class ICPOdometry : public OdometryROS
 			cloud_sub_.shutdown();
 			return;
 		}
+
 		cloudReceived_ = true;
 		if(this->isPaused())
 		{

diff --git 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());
 		}
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -546,7 +548,6 @@ class RGBDOdometry : public OdometryROS
 			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 @@ class RGBDOdometry : public OdometryROS
 	void callbackRGBD(
 			const rtabmap_msgs::RGBDImageConstPtr& image)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -591,7 +591,6 @@ class RGBDOdometry : public OdometryROS
 	void callbackRGBDX(
 			const rtabmap_msgs::RGBDImagesConstPtr& images)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(images->rgbd_images.empty())
@@ -616,7 +615,6 @@ class RGBDOdometry : public OdometryROS
 			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 @@ class RGBDOdometry : public OdometryROS
 			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 @@ class RGBDOdometry : public OdometryROS
 			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 @@ class RGBDOdometry : public OdometryROS
 			const rtabmap_msgs::RGBDImageConstPtr& image4,
 			const rtabmap_msgs::RGBDImageConstPtr& image5)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);

diff --git a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
@@ -202,7 +202,7 @@ class RGBDICPOdometry : public OdometryROS
 					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 @@ class RGBDICPOdometry : public OdometryROS
 			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 ||