diff --git a/CMakeLists.txt b/CMakeLists.txt index 0da52adc..97a90dc7 100644 --- a/CMakeLists.txt +++ b/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") diff --git a/include/robot_localization/ekf.hpp b/include/robot_localization/ekf.hpp index deb43b22..16209fbe 100644 --- a/include/robot_localization/ekf.hpp +++ b/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. diff --git a/include/robot_localization/filter_base.hpp b/include/robot_localization/filter_base.hpp index 353702bd..44db4b4b 100644 --- a/include/robot_localization/filter_base.hpp +++ b/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 diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp index 557d62f3..6a58f4f6 100644 --- a/include/robot_localization/ros_filter.hpp +++ b/include/robot_localization/ros_filter.hpp @@ -39,6 +39,7 @@ #include #include #include +#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 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 & 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 topic_subs_; + //! @brief Dictionary of topic diagnostics which monitors input topic quality + std::map 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 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; + + //! @brief Finds the latests applied measurement. + rclcpp::Time findLatestUpdateTime(const TopicTimeMap & map); + + //! @brief The applied measurements per the state variable. + std::vector 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_ + \ No newline at end of file diff --git a/include/robot_localization/topic_diagnostics.hpp b/include/robot_localization/topic_diagnostics.hpp new file mode 100644 index 00000000..64add6aa --- /dev/null +++ b/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 +#include + +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()) + : min_freq_(settings.min_freq), + max_freq_(settings.max_freq), + task_(std::make_unique( + 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 task_; +}; +} // namespace robot_localization + +#endif // ROBOT_LOCALIZATION__TOPIC_DIAGNOSTICS_HPP_ \ No newline at end of file diff --git a/include/robot_localization/ukf.hpp b/include/robot_localization/ukf.hpp index e8207c03..6120266e 100644 --- a/include/robot_localization/ukf.hpp +++ b/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. diff --git a/src/ekf.cpp b/src/ekf.cpp index 601e188c..8bc1577e 100644 --- a/src/ekf.cpp +++ b/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; } } diff --git a/src/filter_base.cpp b/src/filter_base.cpp index 1853ac68..e157f998 100644 --- a/src/filter_base.cpp +++ b/src/filter_base.cpp @@ -192,7 +192,7 @@ 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_ << @@ -200,6 +200,7 @@ void FilterBase::processMeasurement(const Measurement & measurement) 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( diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index d8e09335..21d29b8a 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -43,6 +43,7 @@ #include #include #include +#include "robot_localization/filter_common.hpp" #include "angles/angles.h" #include "diagnostic_msgs/msg/diagnostic_status.hpp" @@ -105,13 +106,16 @@ RosFilter::RosFilter(const rclcpp::NodeOptions & options) latest_control_(), process_noise_covariance_(STATE_SIZE, STATE_SIZE), initial_estimate_error_covariance_(STATE_SIZE, STATE_SIZE), - last_diag_time_(0, 0, RCL_ROS_TIME), last_published_stamp_(0, 0, RCL_ROS_TIME), predict_to_current_time_(false), last_set_pose_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME), tf_timeout_(0ns), - tf_time_offset_(0ns) + tf_time_offset_(0ns), + use_state_lock_protection_(false), + state_lock_protection_threshold_(0,0), + state_element_update_times_(STATE_SIZE), + state_lock_protection_variance_(Eigen::VectorXd::Constant(STATE_SIZE, 1.0e9)) { tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); @@ -167,7 +171,6 @@ void RosFilter::reset() // Also set the last set pose time, so we ignore all messages // that occur before it last_set_pose_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - last_diag_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); latest_control_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); last_published_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -184,6 +187,8 @@ void RosFilter::reset() filter_.setSensorTimeout(sensor_timeout_); filter_.setProcessNoiseCovariance(process_noise_covariance_); filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); + + state_element_update_times_.resize(STATE_SIZE); } template @@ -543,6 +548,8 @@ void RosFilter::imuCallback( return; } + register_topic_tick(topic_name, msg->header.stamp); + // As with the odometry message, we can separate out the pose- and // twist-related variables in the IMU message and pass them to the pose and // twist callbacks (filters) @@ -712,7 +719,11 @@ void RosFilter::integrateMeasurements(const rclcpp::Time & current_time) } // This will call predict and, if necessary, correct - filter_.processMeasurement(*(measurement.get())); + auto is_measurement_applied = filter_.processMeasurement(*(measurement.get())); + if(is_measurement_applied) + { + updateStateLockProtection(*measurement); + } // Store old states and measurements if we're smoothing if (smooth_lagged_data_) { @@ -764,6 +775,11 @@ void RosFilter::integrateMeasurements(const rclcpp::Time & current_time) last_update_delta); } + if(use_state_lock_protection_) + { + preventStateLock(filter_.getLastMeasurementTime()); + } + RF_DEBUG("\n----- /RosFilter::integrateMeasurements ------\n"); } @@ -792,6 +808,64 @@ void RosFilter::differentiateMeasurements(const rclcpp::Time & current_time) } } +template +void RosFilter::updateStateLockProtection(const Measurement& measurement){ + for(size_t i = 0; i < STATE_SIZE; ++i){ + if(measurement.update_vector_[i] == true){ + auto & data = state_element_update_times_[i][measurement.topic_name_]; + data.time_=measurement.time_; + data.measurement_=measurement.measurement_(i); + data.covariance_=measurement.covariance_(i, i); + } + } +} + +template +rclcpp::Time RosFilter::findLatestUpdateTime(const TopicTimeMap& map){ + auto max_element = std::max_element( + map.begin(), map.end(), + [](const auto& a, const auto& b){ + return a.second.time_ < b.second.time_; + } + ); + + return max_element != map.end() + ? max_element->second.time_ + : rclcpp::Time(0, 0, RCL_ROS_TIME); +} + +template +void RosFilter::preventStateLock(rclcpp::Time state_stamp){ + auto measurement_covariance = filter_.getEstimateErrorCovariance(); + bool state_lock_hazard = false; + auto no_update_duration = rclcpp::Duration(0,0); + // Check if any of the protected state elements have not been updated for a long time + // If so, set the covariance to a high value to prevent Mahalanobis lock + for(auto member : sl_protected_members){ + auto last_update = findLatestUpdateTime(state_element_update_times_[static_cast(member)]); + auto diff = state_stamp - last_update; + no_update_duration = std::max(no_update_duration, diff); + if(diff > state_lock_protection_threshold_){ + state_lock_hazard = true; + auto i = static_cast(member); + measurement_covariance(i,i) = std::max(state_lock_protection_variance_[i], measurement_covariance(i,i)); + } + } + + if(state_lock_hazard) + { + using namespace std::chrono_literals; + filter_.setEstimateErrorCovariance(measurement_covariance); + + auto& clk = *this->get_clock(); + RCLCPP_WARN_THROTTLE( + this->get_logger(), clk, 30'000, // 30s + "Filter did not receive absolute measurement for %0.2f s which exceeds threshold of %0.2f s.\nResetting covariance to prevent Mahalanobis lock!", + no_update_duration.seconds(), state_lock_protection_threshold_.seconds() + ); + } +} + template void RosFilter::loadParams() { @@ -1265,6 +1339,8 @@ void RosFilter::loadParams() this->create_subscription( odom_topic, custom_qos, odom_callback)); + + add_topic_diagnostic_if_configured(odom_topic_name, odom_topic); } else { std::stringstream stream; stream << odom_topic << " is listed as an input topic, but all update " @@ -1404,6 +1480,8 @@ void RosFilter::loadParams() geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, custom_qos, pose_callback)); + add_topic_diagnostic_if_configured(pose_topic_name, pose_topic); + if (differential) { twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; twist_var_counts[StateMemberVy] += pose_update_vec[StateMemberY]; @@ -1498,6 +1576,7 @@ void RosFilter::loadParams() this->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( twist_topic, custom_qos, twist_callback)); + add_topic_diagnostic_if_configured(twist_topic_name, twist_topic); twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVy]; @@ -1706,6 +1785,8 @@ void RosFilter::loadParams() topic_subs_.push_back( this->create_subscription( imu_topic, custom_qos, imu_callback)); + + add_topic_diagnostic_if_configured(imu_topic_name, imu_topic); } else { RCLCPP_ERROR_STREAM( get_logger(), @@ -1881,8 +1962,70 @@ void RosFilter::loadParams() RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); } + + use_state_lock_protection_ = this->declare_parameter("use_state_lock_protection", false); + state_lock_protection_threshold_ = rclcpp::Duration::from_seconds( + this->declare_parameter("state_lock_protection_threshold_s", 15.0)); + + this->declare_parameter("state_lock_protection_variance", rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector spv; + if (this->get_parameter("state_lock_protection_variance", spv)) + { + assert(spv.size() == STATE_SIZE); + for (int i = 0; i < STATE_SIZE; i++) { + state_lock_protection_variance_[i] =spv[i]; + } + } +} + +template +std::optional RosFilter::load_topic_diagnostics_config( + const std::string& topic_name) +{ + auto use_topic_diag = this->declare_parameter( + topic_name + std::string("_topic_diagnostics"), + false); + if (use_topic_diag) { + TopicDiagnosticSettings settings; + settings.min_freq = this->declare_parameter( + topic_name + std::string("_min_frequency"), 10.0); + settings.max_freq = this->declare_parameter( + topic_name + std::string("_max_frequency"), 20.0); + settings.tolerance = this->declare_parameter( + topic_name + std::string("_tolerance"), 0.1); + settings.window_size = this->declare_parameter( + topic_name + std::string("_window_size"), 5); + settings.min_stamp_dt_acceptable = this->declare_parameter( + topic_name + std::string("_min_dt"), -1.0); + settings.max_stamp_dt_acceptable = this->declare_parameter( + topic_name + std::string("_max_dt"), 5.0); + return settings; + } + return {}; } +template +void RosFilter::add_topic_diagnostic_if_configured(const std::string& topic_name, const std::string& diag_name){ + auto settings = load_topic_diagnostics_config(topic_name); + if(settings.has_value()){ + topic_diagnostics_.emplace( + std::piecewise_construct, + std::forward_as_tuple(topic_name), + std::forward_as_tuple( + diag_name, + *this->diagnostic_updater_, + settings.value(), + this->get_clock())); + } +} +template +void RosFilter::register_topic_tick(std::string topic_name, const rclcpp::Time& time) +{ + auto diag = topic_diagnostics_.find(topic_name); + if(diag != topic_diagnostics_.end()){ + diag->second.tick(time); + } +} template void RosFilter::odometryCallback( const nav_msgs::msg::Odometry::SharedPtr msg, @@ -1910,6 +2053,8 @@ void RosFilter::odometryCallback( return; } + register_topic_tick(topic_name, msg->header.stamp); + RF_DEBUG( "------ RosFilter::odometryCallback (" << topic_name << ") ------\n") // << "Odometry message:\n" << *msg); @@ -1969,6 +2114,8 @@ void RosFilter::poseCallback( return; } + register_topic_tick(topic_name, msg->header.stamp); + RF_DEBUG( "------ RosFilter::poseCallback (" << topic_name << ") ------\n" "Pose message:\n" << geometry_msgs::msg::to_yaml(*msg)); @@ -2063,6 +2210,10 @@ void RosFilter::initialize() &RosFilter::aggregateDiagnostics); } + diagnostic_updater_->add( + "State lock protection", this, + &RosFilter::stateLockProtectionDiagnostics); + // Set up the frequency diagnostic min_frequency_ = frequency_ - 2; max_frequency_ = frequency_ + 2; @@ -2074,8 +2225,6 @@ void RosFilter::initialize() &min_frequency_, &max_frequency_, 0.1, 10)); - last_diag_time_ = this->now(); - // Clear out the transforms world_base_link_trans_msg_.transform = tf2::toMsg(tf2::Transform::getIdentity()); @@ -2263,20 +2412,6 @@ void RosFilter::periodicUpdate() accel_pub_->publish(std::move(filtered_acceleration)); } - /* Diagnostics can behave strangely when playing back from bag - * files and using simulated time, so we have to check for - * time suddenly moving backwards as well as the standard - * timeout criterion before publishing. */ - - double diag_duration = (cur_time - last_diag_time_).nanoseconds(); - if (print_diagnostics_ && - (diag_duration >= diagnostic_updater_->getPeriod().nanoseconds() || - diag_duration < 0.0)) - { - diagnostic_updater_->force_update(); - last_diag_time_ = cur_time; - } - // Clear out expired history data if (smooth_lagged_data_) { clearExpiredHistory(filter_.getLastMeasurementTime() - history_length_); @@ -2412,6 +2547,8 @@ void RosFilter::twistCallback( return; } + register_topic_tick(topic_name, msg->header.stamp); + RF_DEBUG( "------ RosFilter::twistCallback (" << topic_name << ") ------\n" "Twist message:\n" << geometry_msgs::msg::to_yaml(*msg)); @@ -2557,6 +2694,41 @@ void RosFilter::aggregateDiagnostics( dynamic_diag_error_level_ = diagnostic_msgs::msg::DiagnosticStatus::OK; } +template +void RosFilter::stateLockProtectionDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & wrapper){ + wrapper.add("State lock protection enabled", use_state_lock_protection_); + wrapper.add("EKF state variable", "Update delay [s]."); + auto isValid = true; + auto filter_stamp = filter_.getLastMeasurementTime(); + for (int i=0; i state_lock_protection_threshold_ && is_protected){ + isValid = false; + } + } + + for (auto & topic_update_time: state_element_update_times_[i]){ + auto & data = topic_update_time.second; + wrapper.add( + "\t" + topic_update_time.first, + "diff time: " + std::to_string((filter_stamp - data.time_).seconds()) + + ", measurement: " + std::to_string(data.measurement_) + + ", covariance: " + std::to_string(data.covariance_) + ); + } + } + + if(isValid){ + wrapper.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else{ + wrapper.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "State lock hazard detected! Reseting state covariance to avoid state lock."); + } +} + template void RosFilter::copyCovariance( const double * arr, Eigen::MatrixXd & covariance, diff --git a/src/ukf.cpp b/src/ukf.cpp index 1d1338d5..6ee655c9 100644 --- a/src/ukf.cpp +++ b/src/ukf.cpp @@ -71,7 +71,7 @@ void Ukf::setConstants(double alpha, double kappa, double beta) } } -void Ukf::correct(const Measurement & measurement) +bool Ukf::correct(const Measurement & measurement) { FB_DEBUG( "---------------------- Ukf::correct ----------------------\n" << @@ -304,6 +304,9 @@ void Ukf::correct(const Measurement & measurement) state_ << "\nCorrected full estimate error covariance is:\n" << estimate_error_covariance_ << "\n\n---------------------- /Ukf::correct ----------------------\n"); + return true; + } else{ + return false; } } diff --git a/test/test_filter_base.cpp b/test/test_filter_base.cpp index 54ba5ff8..3b912885 100644 --- a/test/test_filter_base.cpp +++ b/test/test_filter_base.cpp @@ -54,7 +54,7 @@ class FilterDerived : public FilterBase FilterDerived() : val(0) {} - void correct(const Measurement & measurement) + bool correct(const Measurement & measurement) { EXPECT_EQ(val, measurement.time_); EXPECT_EQ(measurement.topic_name_, "topic"); @@ -63,6 +63,7 @@ class FilterDerived : public FilterBase for (size_t i = 0; i < measurement.update_vector_.size(); ++i) { EXPECT_EQ(measurement.update_vector_[i], true); } + return true; } void predict( const rclcpp::Time & /*reference_time*/, @@ -74,15 +75,17 @@ class FilterDerived2 : public FilterBase public: FilterDerived2() {} - void correct(const Measurement & /*measurement*/) {} + bool correct(const Measurement & /*measurement*/) { + return true; + } void predict( const rclcpp::Time & /*reference_time*/, const rclcpp::Duration & /*delta*/) {} - void processMeasurement(const Measurement & measurement) + bool processMeasurement(const Measurement & measurement) { - FilterBase::processMeasurement(measurement); + return FilterBase::processMeasurement(measurement); } }; diff --git a/test/test_lsp_ekf.cpp b/test/test_lsp_ekf.cpp new file mode 100644 index 00000000..97ede666 --- /dev/null +++ b/test/test_lsp_ekf.cpp @@ -0,0 +1,262 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include + +#include "robot_localization/ekf.hpp" +#include "robot_localization/measurement.hpp" +#include "robot_localization/ros_filter_types.hpp" + +using robot_localization::Measurement; +using robot_localization::Ekf; +using robot_localization::RosEkf; +using robot_localization::STATE_SIZE; + +Measurement createMeasurement(std::string topic_name, rclcpp::Time time, Eigen::VectorXd measurement, Eigen::MatrixXd covariance, std::vector update_vector, double mahalanobis_thresh = 2.0) { + Measurement m; + m.topic_name_ = std::move(topic_name); + m.measurement_ = std::move(measurement); + m.covariance_ = std::move(covariance); + m.update_vector_ = std::move(update_vector); + m.time_ = std::move(time); + m.mahalanobis_thresh_ = mahalanobis_thresh; + return m; +} + +Measurement create_position_measurement(std::string topic_name, rclcpp::Time time, Eigen::Vector3d position, double pos_var) { + Eigen::VectorXd measurement = Eigen::VectorXd::Zero(STATE_SIZE); + measurement(0) = position.x(); + measurement(1) = position.y(); + measurement(2) = position.z(); + + Eigen::MatrixXd cov = Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE); + cov *= 1e9; + cov(0,0) = pos_var; + cov(1,1) = pos_var; + cov(2,2) = pos_var; + + std::vector pos_update_vec{ + true, true, true, + false, false, false, + false, false, false, + false, false, false, + false, false, false + }; + + return createMeasurement(topic_name, time, measurement, cov, pos_update_vec); +} + +TEST(LSP_EkfTest, EkfCorrectShouldReturnFalseForOutlier) { + Eigen::MatrixXd initialCov(15, 15); + initialCov.setIdentity(); + initialCov *= 0.25; + + Ekf filter; + filter.setEstimateErrorCovariance(initialCov); + + { // Initialize the filter + Measurement init_m = create_position_measurement("odom0", rclcpp::Time(1000), Eigen::Vector3d(0.0, 0.0, 0.0), 0.25); + + filter.processMeasurement(init_m); + } + + { // Create a measurement that is valid + Measurement valid_m = create_position_measurement("odom0", rclcpp::Time(1001), Eigen::Vector3d(0.0, 0.0, 0.0), 0.25); + auto is_applied = filter.processMeasurement(valid_m); + EXPECT_TRUE(is_applied); + } + + { // Create an invalid measurement + Measurement invalid_m = create_position_measurement("odom0", rclcpp::Time(1000), Eigen::Vector3d(100.0, 0.0, 0.0), 0.25); + auto is_applied = filter.processMeasurement(invalid_m); + EXPECT_FALSE(is_applied); + } +} + +std::vector create_parameters_override(bool use_state_lock_protection){ + std::vector params; + + Eigen::MatrixXd initialCov = Eigen::MatrixXd::Identity(15, 15); + initialCov *= 0.25; + std::vector initialCovVec(initialCov.data(), initialCov.data() + initialCov.size()); + + params.emplace_back("initial_estimate_covariance", initialCovVec); + params.emplace_back("predict_to_current_time", true); + + std::vector state_lock_var = {1.e8, 1.e9, 1.e10, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9, 1.e9}; + params.emplace_back("use_state_lock_protection", use_state_lock_protection); + params.emplace_back("state_lock_protection_threshold_s", 2.0); + params.emplace_back("state_lock_protection_variance", state_lock_var); + + return params; +} + +TEST(LSP_EkfTest, LockProtectionEnabled_RejectedMeasurementShouldApplyStateLockProtection) { + using namespace std::chrono_literals; + auto time = [](std::chrono::nanoseconds ns) { + return rclcpp::Time(ns.count()); + }; + // node handle is created as per ros2 + rclcpp::NodeOptions options; + options.arguments({"ekf_filter_node"}); + options.parameter_overrides(create_parameters_override(true)); + + std::shared_ptr filter = + std::make_shared(options); + filter->initialize(); + + { // Initialize the filter + Measurement init_m = create_position_measurement("odom0", time(1s), Eigen::Vector3d(0.0, 0.0, 0.0), 100.0); + filter->enqueueMeasurement(init_m.topic_name_, init_m.measurement_, init_m.covariance_, init_m.update_vector_, init_m.mahalanobis_thresh_, init_m.time_); + } + + { // Create a measurement that is valid + Measurement valid_m = create_position_measurement("odom0", time(2s), Eigen::Vector3d(1.0, 2.0, 0.0), 0.1*0.1); + filter->enqueueMeasurement(valid_m.topic_name_, valid_m.measurement_, valid_m.covariance_, valid_m.update_vector_, valid_m.mahalanobis_thresh_, valid_m.time_); + } + + { // Create an invalid measurement + Measurement invalid_m = create_position_measurement("odom0", time(3s), Eigen::Vector3d(100.0, 0.0, 0.0), 0.25); + filter->enqueueMeasurement(invalid_m.topic_name_, invalid_m.measurement_, invalid_m.covariance_, invalid_m.update_vector_, invalid_m.mahalanobis_thresh_, invalid_m.time_); + } + + filter->robot_localization::RosEkf::integrateMeasurements(time(5s)); + + auto state = filter->getFilter().getState(); + EXPECT_NEAR(state[0], 1.0, 0.1); + EXPECT_NEAR(state[1], 2.0, 0.1); + + auto cov = filter->getFilter().getEstimateErrorCovariance(); + + EXPECT_DOUBLE_EQ(cov(0,0), 1.e8); + EXPECT_DOUBLE_EQ(cov(1,1), 1.e9); + EXPECT_DOUBLE_EQ(cov(2,2), 1.e10); +}; + +TEST(LSP_EkfTest, LockProtectionEnabled_ValidMeasurementShouldNotApplyStateLockProtection) { + using namespace std::chrono_literals; + auto time = [](std::chrono::nanoseconds ns) { + return rclcpp::Time(ns.count()); + }; + // node handle is created as per ros2 + rclcpp::NodeOptions options; + options.arguments({"ekf_filter_node"}); + options.parameter_overrides(create_parameters_override(true)); + + std::shared_ptr filter = + std::make_shared(options); + filter->initialize(); + + { // Initialize the filter + Measurement init_m = create_position_measurement("odom0", time(1s), Eigen::Vector3d(0.0, 0.0, 0.0), 100.0); + filter->enqueueMeasurement(init_m.topic_name_, init_m.measurement_, init_m.covariance_, init_m.update_vector_, init_m.mahalanobis_thresh_, init_m.time_); + } + + { // Create a measurement that is valid + Measurement valid_m = create_position_measurement("odom0", time(2s), Eigen::Vector3d(0.0, 0.0, 0.0), 0.1*0.1); + filter->enqueueMeasurement(valid_m.topic_name_, valid_m.measurement_, valid_m.covariance_, valid_m.update_vector_, valid_m.mahalanobis_thresh_, valid_m.time_); + } + + { // Create an invalid measurement + Measurement invalid_m = create_position_measurement("odom0", time(3s), Eigen::Vector3d(0.0, 0.0, 0.0), 0.25); + filter->enqueueMeasurement(invalid_m.topic_name_, invalid_m.measurement_, invalid_m.covariance_, invalid_m.update_vector_, invalid_m.mahalanobis_thresh_, invalid_m.time_); + } + + filter->robot_localization::RosEkf::integrateMeasurements(time(5s)); + + auto state = filter->getFilter().getState(); + EXPECT_NEAR(state[0], 0.0, 0.1); + EXPECT_NEAR(state[1], 0.0, 0.1); + + auto cov = filter->getFilter().getEstimateErrorCovariance(); + + EXPECT_LT(cov(0,0), 10.0); + EXPECT_LT(cov(1,1), 10.0); + EXPECT_LT(cov(2,2), 10.0); +}; + + +TEST(LSP_EkfTest, LockProtectionDisabled_ShouldNotApplyStateLockProtection) { + using namespace std::chrono_literals; + auto time = [](std::chrono::nanoseconds ns) { + return rclcpp::Time(ns.count()); + }; + // node handle is created as per ros2 + rclcpp::NodeOptions options; + options.arguments({"ekf_filter_node"}); + options.parameter_overrides(create_parameters_override(false)); + + std::shared_ptr filter = + std::make_shared(options); + filter->initialize(); + + { // Initialize the filter + Measurement init_m = create_position_measurement("odom0", time(1s), Eigen::Vector3d(0.0, 0.0, 0.0), 100.0); + filter->enqueueMeasurement(init_m.topic_name_, init_m.measurement_, init_m.covariance_, init_m.update_vector_, init_m.mahalanobis_thresh_, init_m.time_); + } + + { // Create a measurement that is valid + Measurement valid_m = create_position_measurement("odom0", time(2s), Eigen::Vector3d(1.0, 2.0, 0.0), 0.1*0.1); + filter->enqueueMeasurement(valid_m.topic_name_, valid_m.measurement_, valid_m.covariance_, valid_m.update_vector_, valid_m.mahalanobis_thresh_, valid_m.time_); + } + + { // Create an invalid measurement + Measurement invalid_m = create_position_measurement("odom0", time(3s), Eigen::Vector3d(100.0, 0.0, 0.0), 0.25); + filter->enqueueMeasurement(invalid_m.topic_name_, invalid_m.measurement_, invalid_m.covariance_, invalid_m.update_vector_, invalid_m.mahalanobis_thresh_, invalid_m.time_); + } + + filter->robot_localization::RosEkf::integrateMeasurements(time(5s)); + + auto state = filter->getFilter().getState(); + EXPECT_NEAR(state[0], 1.0, 0.1); + EXPECT_NEAR(state[1], 2.0, 0.1); + + auto cov = filter->getFilter().getEstimateErrorCovariance(); + + EXPECT_LT(cov(0,0), 20.0); + EXPECT_LT(cov(1,1), 20.0); + EXPECT_LT(cov(2,2), 20.0); +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return ret; +}