Removed composite task to avoid empty msg, added odom status task

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -38,6 +38,8 @@ 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>
@@ -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<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_;
 };
 
 }

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<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
@@ -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<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, 
+			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.");
+	}
+}
 
 }

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)

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)

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -11,37 +11,38 @@ 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)
     {}
 
 protected:
     void initDiagnostic(
-        const std::string & imageTopic,
-        const std::string & topicsNotReceivedWarningMsg)
+        const std::string & topic,
+        const std::string & topicsNotReceivedWarningMsg,
+		std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
     {
         topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
 
-		std::list<std::string> strList = uSplit(imageTopic, '/');
+		std::list<std::string> 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; i<otherTasks.size(); ++i)
+		{
+			diagnosticUpdater_.add(*otherTasks[i]);
+		}
 		diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
-		diagnosticUpdater_.add(compositeTask_);
 		diagnosticUpdater_.force_update();
 		diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
     }
 
     void tick(const ros::Time & stamp, double targetFrequency = 0)
     {
         frequencyStatus_.tick();
-		timeStampStatus_.tick(stamp.toSec());
 		double period = stamp.toSec() - lastCallbackCalledStamp_;
 		if(period>0.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_ */
+#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */