Added sync diagnotic

introlab · Aug 26, 2023 · cd61b7b · cd61b7b
1 parent cd52f76
commit cd61b7b
Show file tree

Hide file tree

Showing 28 changed files with 228 additions and 288 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
@@ -45,14 +45,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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 +78,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);
 
 	virtual void flushCallbacks() = 0;
 	tf::TransformListener & tfListener() {return tfListener_;}
@@ -88,7 +88,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 +97,6 @@ class OdometryROS : public nodelet::Nodelet
 
 private:
 	rtabmap::Odometry * odometry_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;

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,17 @@ void OdometryROS::onInit()
 	onOdomInit();
 }
 
-void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
+void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync)
 {
-	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 "
+	initDiagnostic("",
+		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()));
 }
 
 rtabmap::Transform OdometryROS::velocityGuess() const
@@ -953,6 +933,16 @@ 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());
 		}
+
+		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();
 	}
 }

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
@@ -364,7 +364,7 @@ class RGBDOdometry : public OdometryROS
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
 		}
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -546,7 +546,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 +574,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 +589,6 @@ class RGBDOdometry : public OdometryROS
 	void callbackRGBDX(
 			const rtabmap_msgs::RGBDImagesConstPtr& images)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(images->rgbd_images.empty())
@@ -616,7 +613,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 +632,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 +654,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 +679,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 ||

diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp
@@ -282,7 +282,7 @@ class StereoOdometry : public OdometryROS
 					cameraInfoRight_.getTopic().c_str());
 		}
 
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -578,7 +578,6 @@ class StereoOdometry : public OdometryROS
 				const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 				const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -609,7 +608,6 @@ class StereoOdometry : public OdometryROS
 	void callbackRGBD(
 			const rtabmap_msgs::RGBDImageConstPtr& image)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -627,7 +625,6 @@ class StereoOdometry : public OdometryROS
 	void callbackRGBDX(
 			const rtabmap_msgs::RGBDImagesConstPtr& images)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(images->rgbd_images.empty())
@@ -654,7 +651,6 @@ class StereoOdometry : public OdometryROS
 			const rtabmap_msgs::RGBDImageConstPtr& image,
 			const rtabmap_msgs::RGBDImageConstPtr& image2)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -677,7 +673,6 @@ class StereoOdometry : public OdometryROS
 			const rtabmap_msgs::RGBDImageConstPtr& image2,
 			const rtabmap_msgs::RGBDImageConstPtr& image3)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -704,7 +699,6 @@ class StereoOdometry : public OdometryROS
 			const rtabmap_msgs::RGBDImageConstPtr& image3,
 			const rtabmap_msgs::RGBDImageConstPtr& image4)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);

diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp
@@ -2245,6 +2245,12 @@ void CoreWrapper::process(
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
+
+		// If not intermediate node
+		if(data.id() > 0)
+		{
+			tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
+		}
 	}
 	else if(!rtabmap_.isIDsGenerated())
 	{

diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt
@@ -3,7 +3,7 @@ project(rtabmap_sync)
 
 find_package(catkin REQUIRED COMPONENTS
             cv_bridge roscpp sensor_msgs nav_msgs image_transport
-            nodelet message_filters rtabmap_msgs rtabmap_conversions
+            nodelet message_filters rtabmap_msgs rtabmap_conversions diagnostic_updater
 )
 
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
@@ -33,7 +33,7 @@ catkin_package(
   INCLUDE_DIRS include
   LIBRARIES rtabmap_sync rtabmap_sync_plugins
   CATKIN_DEPENDS cv_bridge roscpp sensor_msgs nav_msgs image_transport
-                 nodelet message_filters rtabmap_msgs rtabmap_conversions
+                 nodelet message_filters rtabmap_msgs rtabmap_conversions diagnostic_updater
   CFG_EXTRAS extra_configs.cmake
 )
 

diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
@@ -51,12 +51,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/OdomInfo.h>
 #include <rtabmap_msgs/ScanDescriptor.h>
 #include <rtabmap_sync/CommonDataSubscriberDefines.h>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 #include <boost/thread.hpp>
 
 namespace rtabmap_sync {
 
-class CommonDataSubscriber {
+class CommonDataSubscriber : public SyncDiagnostic {
 public:
 	CommonDataSubscriber(bool gui);
 	virtual ~CommonDataSubscriber();
@@ -122,8 +123,6 @@ class CommonDataSubscriber {
 				const cv::Mat & localDescriptors = cv::Mat());
 
 private:
-	void warningLoop();
-	void callbackCalled() {callbackCalled_ = true;}
 	void setupDepthCallbacks(
 			ros::NodeHandle & nh,
 			ros::NodeHandle & pnh,
@@ -256,8 +255,6 @@ class CommonDataSubscriber {
 
 private:
 	bool approxSync_;
-	boost::thread* warningThread_;
-	bool callbackCalled_;
 	bool subscribedToDepth_;
 	bool subscribedToStereo_;
 	bool subscribedToRGB_;