Add topic diagnostics and state lock protection for filter.

CMakeLists.txt

@@ -180,6 +180,14 @@ if(BUILD_TESTING)
     test_ekf_node_interfaces
     ${library_name})
 
+  # --- Lock state protection tests ---
+  ament_add_gtest(test_lsp_ekf test/test_lsp_ekf.cpp)
+  target_link_libraries(test_lsp_ekf ${library_name})
+
+  rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
+  target_link_libraries(test_lsp_ekf "${cpp_typesupport_target}")
+  # --- Lock state protection tests ---
+
   rosidl_get_typesupport_target(
     cpp_typesupport_target
     "${PROJECT_NAME}" "rosidl_typesupport_cpp")

include/robot_localization/ekf.hpp

@@ -65,7 +65,7 @@ class Ekf : public FilterBase
    *
    * @param[in] measurement - The measurement to fuse with our estimate
    */
-  void correct(const Measurement & measurement) override;
+  bool correct(const Measurement & measurement) override;
 
   /**
    * @brief Carries out the predict step in the predict/update cycle.

include/robot_localization/filter_base.hpp

@@ -74,8 +74,10 @@ class FilterBase
    * @brief Carries out the correct step in the predict/update cycle. This
    * method must be implemented by subclasses.
    * @param[in] measurement - The measurement to fuse with the state estimate
+   * 
+   * @return Whether or not the measurement was successfully fused or rejected
    */
-  virtual void correct(const Measurement & measurement) = 0;
+  virtual bool correct(const Measurement & measurement) = 0;
 
   /**
    * @brief Returns the control vector currently being used
@@ -165,7 +167,7 @@ class FilterBase
    * @brief Does some final preprocessing, carries out the predict/update cycle
    * @param[in] measurement - The measurement object to fuse into the filter
    */
-  virtual void processMeasurement(const Measurement & measurement);
+  virtual bool processMeasurement(const Measurement & measurement);
 
   /**
    * @brief Sets the most recent control term

include/robot_localization/ros_filter.hpp

@@ -39,6 +39,7 @@
 #include <queue>
 #include <string>
 #include <vector>
+#include "robot_localization/measurement.hpp"
 
 #include "diagnostic_msgs/msg/diagnostic_status.hpp"
 #include "diagnostic_updater/diagnostic_updater.hpp"
@@ -52,10 +53,12 @@
 #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "robot_localization/filter_common.hpp"
 #include "robot_localization/filter_state.hpp"
 #include "robot_localization/measurement.hpp"
 #include "robot_localization/srv/toggle_filter_processing.hpp"
 #include "robot_localization/srv/set_pose.hpp"
+#include "robot_localization/topic_diagnostics.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "tf2/LinearMath/Transform.h"
@@ -249,6 +252,22 @@ class RosFilter : public rclcpp::Node
   //!
   void loadParams();
 
+  //! @brief Loads \parameters for topic diagnostics
+  std::optional<TopicDiagnosticSettings> load_topic_diagnostics_config(
+    const std::string& topic_name
+  );
+
+  //! @brief Configures a topic diagnostic if diagnostic for topic is enabled
+  //! @param[in] topic_name - The name of the topic in configuration ex. pose0
+  //! @param[in] diag_name - The name displayed in topic diagnostic.
+  //!                        For quick identification, the ROS topic name is used
+  void add_topic_diagnostic_if_configured(
+    const std::string& topic_name,
+    const std::string& diag_name);
+
+  //! @brief Register received message for topic diagnostics
+  void register_topic_tick(std::string topic_name, const rclcpp::Time& time);
+
   //! @brief callback function which is called for periodic updates
   //!
   void periodicUpdate();
@@ -390,6 +409,12 @@ class RosFilter : public rclcpp::Node
   void aggregateDiagnostics(
     diagnostic_updater::DiagnosticStatusWrapper & wrapper);
 
+  //! @brief Publishes diagnostics for state protection algorithm
+  //! @param[in] wrapper - The diagnostic status wrapper to update
+  //!
+  void stateLockProtectionDiagnostics(
+    diagnostic_updater::DiagnosticStatusWrapper & wrapper);
+
   //! @brief Utility method for copying covariances from ROS covariance arrays
   //! to Eigen matrices
   //!
@@ -496,6 +521,18 @@ class RosFilter : public rclcpp::Node
     std::vector<bool> & updateVector, Eigen::VectorXd & measurement,
     Eigen::MatrixXd & measurementCovariance);
 
+  //! @brief Tracks the timestamp of the last update for state variables
+  //! @param[in] measurement - The processed measurement
+  //!
+  void updateStateLockProtection(const Measurement& measurement);
+
+  //! @brief The function checks when the last accepted state variable update occurred. 
+  //!        If the state variable has not been updated for a time exceeding the configured threshold, 
+  //!        the covariance for this variable is set to unknown.
+  //! @param[in] state_stamp - Time stamp of filter state
+  //!
+  void preventStateLock(rclcpp::Time state_stamp);
+
   //! @brief Whether or not we print diagnostic messages to the /diagnostics
   //! topic
   //!
@@ -633,10 +670,6 @@ class RosFilter : public rclcpp::Node
   //!
   geometry_msgs::msg::TransformStamped world_base_link_trans_msg_;
 
-  //! @brief last call of periodicUpdate
-  //!
-  rclcpp::Time last_diag_time_;
-
   //! @brief The time of the most recent published state
   //!
   rclcpp::Time last_published_stamp_;
@@ -728,6 +761,9 @@ class RosFilter : public rclcpp::Node
   //!
   std::vector<rclcpp::SubscriptionBase::SharedPtr> topic_subs_;
 
+  //! @brief Dictionary of topic diagnostics which monitors input topic quality
+  std::map<std::string, TopicDiagnostic> topic_diagnostics_;
+
   //! @brief Stores the last measurement from a given topic for differential
   //! integration
   //!
@@ -854,8 +890,49 @@ class RosFilter : public rclcpp::Node
   //! Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam
   //!
   double max_frequency_;
-};
 
-}  // namespace robot_localization
+  //! @brief Indicates that we should monitor the applied measurements and reset the covariance of
+  //! the state variable didn't updated with them.
+  bool use_state_lock_protection_ = false;
 
-#endif  // ROBOT_LOCALIZATION__ROS_FILTER_HPP_
+  //! @brief State lock protection members
+  //! A list of members which will should be check with state lock protection mechanism
+  //!
+  std::vector<StateMembers> sl_protected_members{
+    StateMemberX,
+    StateMemberY,
+    StateMemberZ,
+  };
+
+  //! @brief The time threshold within which the state variable should be updated to not have reset
+  //! covariance.
+  rclcpp::Duration state_lock_protection_threshold_;
+
+  //! @brief Data gathered on the applied measurement for the state lock protection mechanism.
+  struct StateLockMeasurementData
+  {
+    //! @brief Time of the measurement.
+    rclcpp::Time time_;
+    //! @brief Applied measurement.
+    double measurement_;
+    //! @brief Covariance of the applied measurement.
+    double covariance_;
+  };
+
+  //! @brief Type for the map of the input to the state lock protection mechanism data.
+  using TopicTimeMap = std::map<std::string, StateLockMeasurementData>;
+
+  //! @brief Finds the latests applied measurement.
+  rclcpp::Time findLatestUpdateTime(const TopicTimeMap & map);
+
+  //! @brief The applied measurements per the state variable.
+  std::vector<TopicTimeMap> state_element_update_times_;
+
+  //! @brief The variance value to which the state variable covariance will be reset.
+  Eigen::VectorXd state_lock_protection_variance_;
+ };
+
+ }  // namespace robot_localization
+
+ #endif  // ROBOT_LOCALIZATION__ROS_FILTER_HPP_
+

include/robot_localization/topic_diagnostics.hpp

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROBOT_LOCALIZATION__TOPIC_DIAGNOSTICS_HPP_
+#define ROBOT_LOCALIZATION__TOPIC_DIAGNOSTICS_HPP_
+
+#include <memory>
+#include <diagnostic_updater/publisher.hpp>
+
+namespace robot_localization
+{
+
+//! @brief Struct to hold settings for topic diagnostics
+struct TopicDiagnosticSettings
+{
+  //! @brief The minimum frequency for the topic
+  double min_freq = 0.0;
+  //! @brief The maximum frequency for the topic
+  double max_freq = 0.0;
+  //! @brief The tolerance for the frequency
+  double tolerance = 0.1;
+  //! @brief The window size for the frequency
+  int window_size = 5;
+  //! @brief The minimum acceptable time difference between messages
+  double min_stamp_dt_acceptable = -1.0;
+  //! @brief The maximum acceptable time difference between messages
+  double max_stamp_dt_acceptable = 5.0;
+};
+
+//! @brief Class to encapsulate \class diagnostic_updater::TopicDiagnostic for more convenient API
+//! The contructor for \class diagnostic_updater::TopicDiagnostic takes parameters by a \class diagnostic_updater::FrequencyStatusParam
+//! which stores the min and max frequency as pointers, what is inconvenient. 
+//! This class provides storage for min and max frequency and passes them to \class diagnostic_updater::TopicDiagnostic
+class TopicDiagnostic final
+{
+public:
+  TopicDiagnostic(
+    std::string name, diagnostic_updater::Updater & diag, const TopicDiagnosticSettings & settings,
+    const rclcpp::Clock::SharedPtr & clock = std::make_shared<rclcpp::Clock>())
+  : min_freq_(settings.min_freq),
+    max_freq_(settings.max_freq),
+    task_(std::make_unique<diagnostic_updater::TopicDiagnostic>(
+      name, diag,
+      diagnostic_updater::FrequencyStatusParam(
+        &min_freq_, &max_freq_, settings.tolerance, settings.window_size),
+      diagnostic_updater::TimeStampStatusParam{
+        settings.min_stamp_dt_acceptable, settings.max_stamp_dt_acceptable},
+      clock))
+  {
+  }
+
+  TopicDiagnostic(const TopicDiagnostic &) = delete;
+  TopicDiagnostic(TopicDiagnostic &&) = default;
+  TopicDiagnostic & operator=(const TopicDiagnostic &) = delete;
+  TopicDiagnostic & operator=(TopicDiagnostic &&) = default;
+
+  void tick(const rclcpp::Time & stamp) { task_->tick(stamp); }
+
+private:
+  double min_freq_ = 0.0;
+  double max_freq_ = 0.0;
+  std::unique_ptr<diagnostic_updater::TopicDiagnostic> task_;
+};
+}  // namespace robot_localization
+
+#endif  // ROBOT_LOCALIZATION__TOPIC_DIAGNOSTICS_HPP_

include/robot_localization/ukf.hpp

@@ -80,7 +80,7 @@ class Ukf : public FilterBase
    *
    * @param[in] measurement - The measurement to fuse with our estimate
    */
-  void correct(const Measurement & measurement) override;
+  bool correct(const Measurement & measurement) override;
 
   /**
    * @brief  Carries out the predict step in the predict/update cycle.

src/ekf.cpp

@@ -51,7 +51,7 @@ Ekf::Ekf()
 
 Ekf::~Ekf() {}
 
-void Ekf::correct(const Measurement & measurement)
+bool Ekf::correct(const Measurement & measurement)
 {
   FB_DEBUG(
     "---------------------- Ekf::correct ----------------------\n" <<
@@ -216,6 +216,9 @@ void Ekf::correct(const Measurement & measurement)
         state_ << "\nCorrected full estimate error covariance is:\n" <<
         estimate_error_covariance_ <<
         "\n\n---------------------- /Ekf::correct ----------------------\n");
+    return true;
+  } else{
+    return false;
   }
 }
 

src/filter_base.cpp

@@ -192,14 +192,15 @@ const rclcpp::Duration & FilterBase::getSensorTimeout()
 
 const Eigen::VectorXd & FilterBase::getState() {return state_;}
 
-void FilterBase::processMeasurement(const Measurement & measurement)
+bool FilterBase::processMeasurement(const Measurement & measurement)
 {
   FB_DEBUG(
     "------ FilterBase::processMeasurement (" << measurement.topic_name_ <<
       ") ------\n");
 
   rclcpp::Duration delta(0, 0u);
 
+  auto is_measurement_applied = false;
   // If we've had a previous reading, then go through the predict/update
   // cycle. Otherwise, set our state and covariance to whatever we get
   // from this measurement.
@@ -224,7 +225,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
       predicted_state_ = state_;
     }
 
-    correct(measurement);
+    is_measurement_applied = correct(measurement);
   } else {
     FB_DEBUG("First measurement. Initializing filter.\n");
 
@@ -245,6 +246,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
       }
     }
 
+    is_measurement_applied = true;
     initialized_ = true;
   }
 
@@ -262,6 +264,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
   FB_DEBUG(
     "------ /FilterBase::processMeasurement (" << measurement.topic_name_ <<
       ") ------\n");
+  return is_measurement_applied;
 }
 
 void FilterBase::setControl(