Removal of dead code and better debug output

doc/state_estimation_nodes.rst

@@ -242,6 +242,10 @@ If any of your sensors produce data with timestamps that are older than the most
 ^^^^^^^^^^^^^^^
 If ``smooth_lagged_data`` is set to *true*, this parameter specifies the number of seconds for which the filter will retain its state and measurement history. This value should be at least as large as the time delta between your lagged measurements and the current time.
 
+~max_future_queue_size
+^^^^^^^^^^^^^^^
+This parameter specifies the number of total data samples, arriving between filter runs, the filter will retain. This is to ensure excessive memory is not used as would happen if incoming data is badly time stamped or the filter ``frequency`` is badly set. 
+
 ~[sensor]_nodelay
 ^^^^^^^^^^^^^^^^^
 

include/robot_localization/ros_filter.hpp

@@ -473,6 +473,25 @@ class RosFilter : public rclcpp::Node
     std::vector<bool> & updateVector, Eigen::VectorXd & measurement,
     Eigen::MatrixXd & measurementCovariance);
 
+  //! @brief Validates incoming measurement ordering and produces errors if
+  //! validation does not pass.
+  //! @param[in] topicName - The name of the topic over which this message was
+  //! received
+  //! @param[in] stamp - The timestamp associated with this message.
+  //! @return true if the measrement is newer than all known measurements.
+  //!
+  bool validateMeasurementOrdering(const std::string& topicName,
+                                   const builtin_interfaces::msg::Time& stamp);
+
+  //! @brief Validates incoming measurements are within a reasonable timeframe.
+  //! @param[in] topicName - The name of the topic over which this message was
+  //! received
+  //! @param[in] stamp - The timestamp associated with this message.
+  //! @return true if the measrement is newer than all known measurements.
+  //!
+  bool validateMeasurementTime(const std::string& topicName,
+                               const builtin_interfaces::msg::Time& stamp);
+
   //! @brief Whether or not we print diagnostic messages to the /diagnostics
   //! topic
   //!
@@ -545,14 +564,17 @@ class RosFilter : public rclcpp::Node
   //!
   double gravitational_acceleration_;
 
-  //! @brief The depth of the history we track for smoothing/delayed measurement
-  //! processing
+  //! @brief The depth of the history we track for smoothing/delayed
+  //! measurement processing
   //!
   //! This is the guaranteed minimum buffer size for which previous states and
   //! measurements are kept.
   //!
   rclcpp::Duration history_length_;
 
+  //! @brief This is the maximum measurement queque length.
+  uint64_t max_future_queue_size_;
+
   //! @brief tf frame name for the robot's body frame
   //!
   std::string base_link_frame_id_;
@@ -622,8 +644,8 @@ class RosFilter : public rclcpp::Node
   //! @brief This object accumulates static diagnostics, e.g., diagnostics
   //! relating to the configuration parameters.
   //!
-  //! The values are treated as static and always reported (i.e., this object is
-  //! never cleared)
+  //! The values are treated as static and always reported (i.e., this object
+  //! is never cleared)
   //!
   std::map<std::string, std::string> static_diagnostics_;
 
@@ -646,8 +668,8 @@ class RosFilter : public rclcpp::Node
   //! stores the initial measurements. Note that this is different from using
   //! differential mode, as in differential mode, pose data is converted to
   //! twist data, resulting in boundless error growth for the variables being
-  //! fused. With relative measurements, the vehicle will start with a 0 heading
-  //! and position, but the measurements are still fused absolutely.
+  //! fused. With relative measurements, the vehicle will start with a 0
+  //! heading and position, but the measurements are still fused absolutely.
   std::map<std::string, tf2::Transform> initial_measurements_;
 
   //! @brief If including acceleration for each IMU input, whether or not we
@@ -688,9 +710,9 @@ class RosFilter : public rclcpp::Node
   //!
   std::map<std::string, Eigen::MatrixXd> previous_measurement_covariances_;
 
-  //! @brief By default, the filter predicts and corrects up to the time of the
-  //! latest measurement. If this is set to true, the filter does the same, but
-  //! then also predicts up to the current time step.
+  //! @brief By default, the filter predicts and corrects up to the time of
+  //! the latest measurement. If this is set to true, the filter does the
+  //! same, but then also predicts up to the current time step.
   //!
   bool predict_to_current_time_;
 

src/navsat_transform.cpp

@@ -83,6 +83,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
   yaw_offset_(0.0),
   zero_altitude_(false)
 {
+
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);