Added localization diagnostic when loc_thr parameter is set.

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -399,6 +399,19 @@ private:
 	ros::Time previousStamp_;
 
 	rtabmap_util::ULogToRosout ulogToRosout_;
+
+	class LocalizationStatusTask : public diagnostic_updater::DiagnosticTask
+	{
+	public:
+		LocalizationStatusTask();
+		void setLocalizationThreshold(double value);
+		void updateStatus(const cv::Mat & covariance, bool twoDMapping);
+		void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
+	private:
+		double localizationThreshold_;
+		double localizationError_;
+	};
+	LocalizationStatusTask localizationDiagnostic_;
 };
 
 }