merged master->ros2

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -156,6 +156,11 @@ private:
 			const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
 			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg);
 
+	virtual void commonSensorDataCallback(
+			const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg);
+
 	void defaultCallback(const sensor_msgs::msg::Image::ConstSharedPtr imageMsg); // no odom
 
 	void userDataAsyncCallback(const rtabmap_msgs::msg::UserData::SharedPtr dataMsg);
@@ -417,6 +422,19 @@ private:
 	rclcpp::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_;
 };
 
 }