Added localization diagnostic when loc_thr parameter is set.

rtabmap_launch/launch/rtabmap.launch

@@ -28,6 +28,7 @@
   <!-- Localization-only mode -->
   <arg name="localization"            default="false"/>
   <arg name="initial_pose"            default=""/>             <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->
+  <arg name="loc_thr"                 default="0.0"/>          <!-- In localization mode, setting this threshold will make rtabmap publishing diagnostics about if the robot is localized (under that error threshold). -->
 
   <!-- sim time for convenience, if playing a rosbag -->
   <arg name="use_sim_time"            default="false"/>
@@ -366,6 +367,7 @@
       <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
       <param name="initial_pose"         type="string" value="$(arg initial_pose)"/>
       <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
+      <param name="loc_thr"              type="double" value="$(arg loc_thr)"/>
       <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
       <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
       <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>

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_;
 };
 
 }

rtabmap_slam/src/CoreWrapper.cpp

@@ -742,7 +742,16 @@ void CoreWrapper::onInit()
 				Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
 	}
 
-	setupCallbacks(nh, pnh, getName()); // do it at the end
+	std::vector<diagnostic_updater::DiagnosticTask*> tasks;
+	double localizationThreshold = 0.0f;
+	pnh.param("loc_thr", localizationThreshold, localizationThreshold);
+	if(rtabmap_.getMemory() && !rtabmap_.getMemory()->isIncremental() && localizationThreshold > 0.0)
+	{
+		NODELET_INFO("rtabmap: loc_thr  = %f", localizationThreshold);
+		localizationDiagnostic_.setLocalizationThreshold(localizationThreshold);
+		tasks.push_back(&localizationDiagnostic_);
+	}
+	setupCallbacks(nh, pnh, getName(), tasks); // do it at the end
 	if(!this->isDataSubscribed())
 	{
 		bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
@@ -2224,6 +2233,13 @@ void CoreWrapper::process(
 
 				timePublishMaps = timer.ticks();
 			}
+
+			// If not intermediate node
+			if(data.id() > 0)
+			{
+				localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_);
+				tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
+			}
 		}
 		else
 		{
@@ -2245,12 +2261,6 @@ 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())
 	{
@@ -4475,6 +4485,50 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
 	}
 }
 
+CoreWrapper::LocalizationStatusTask::LocalizationStatusTask() :
+		diagnostic_updater::DiagnosticTask("Localization status"),
+		localizationThreshold_(0.0),
+		localizationError_(9999)
+{}
+
+void CoreWrapper::LocalizationStatusTask::setLocalizationThreshold(double value)
+{
+	localizationThreshold_ = value;
+}
+
+void CoreWrapper::LocalizationStatusTask::updateStatus(const cv::Mat & cov, bool twoDMapping)
+{
+	if(localizationThreshold_ > 0.0 && !cov.empty())
+	{
+		if(cov.at<double>(0,0) >= 9999.0)
+		{
+			localizationError_ = 9999.0;
+		}
+		else
+		{
+			localizationError_ = sqrt(uMax3(cov.at<double>(0,0), cov.at<double>(1,1), twoDMapping?0.0:cov.at<double>(2,2)));
+		}
+	}
+}
+
+void CoreWrapper::LocalizationStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+	if(localizationError_>=9999)
+	{
+		stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not localized!");
+	}
+	else if(localizationError_ > localizationThreshold_)
+	{
+		stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Localization error is high!");
+	}
+	else
+	{
+		stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Localized.");
+	}
+	stat.add("Localization error (m)", localizationError_);
+	stat.add("loc_thr (m)", localizationThreshold_);
+}
+
 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
 bool CoreWrapper::octomapBinaryCallback(

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h

@@ -80,7 +80,8 @@ protected:
 	void setupCallbacks(
 			ros::NodeHandle & nh,
 			ros::NodeHandle & pnh,
-			const std::string & name);
+			const std::string & name,
+			std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
 	virtual void commonMultiCameraCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_msgs::UserDataConstPtr & userDataMsg,

rtabmap_sync/src/CommonDataSubscriber.cpp

@@ -356,7 +356,8 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 void CommonDataSubscriber::setupCallbacks(
 		ros::NodeHandle & nh,
 		ros::NodeHandle & pnh,
-		const std::string & name)
+		const std::string & name,
+		std::vector<diagnostic_updater::DiagnosticTask*> otherTasks)
 {
 	bool subscribeScan2d = false;
 	bool subscribeScan3d = false;
@@ -670,7 +671,8 @@ void CommonDataSubscriber::setupCallbacks(
 					approxSync_?
 							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
 							"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()),
+					otherTasks);
 		
 	}
 }