Preliminarily decide to use earliest/uninitialized time

Signed-off-by: methylDragon <methylDragon@gmail.com>

fuse_core/include/fuse_core/message_buffer_impl.h

@@ -144,6 +144,7 @@ void MessageBuffer<Message>::purgeHistory()
   if (ending_stamp.seconds() > buffer_length.seconds()) {
     auto expiration_time = ending_stamp - buffer_length;
   } else {
+    // NOTE(CH3): Uninitialized. But okay because it's just used for comparison.
     auto expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type);
   }
 

fuse_models/src/odometry_2d_publisher.cpp

@@ -63,8 +63,8 @@ namespace fuse_models
 Odometry2DPublisher::Odometry2DPublisher() :
   fuse_core::AsyncPublisher(1),
   device_id_(fuse_core::uuid::NIL),
-  latest_stamp_(Synchronizer::TIME_ZERO),
-  latest_covariance_stamp_(Synchronizer::TIME_ZERO),
+  latest_stamp_(Synchronizer::ROS_TIME_ZERO),
+  latest_covariance_stamp_(Synchronizer::ROS_TIME_ZERO),
   publish_timer_spinner_(1, &publish_timer_callback_queue_)
 {
 }
@@ -96,7 +96,7 @@ void Odometry2DPublisher::notifyCallback(
 {
   // Find the most recent common timestamp
   const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph);
-  if (latest_stamp == Synchronizer::TIME_ZERO)
+  if (latest_stamp == Synchronizer::ROS_TIME_ZERO)
   {
     {
       std::lock_guard<std::mutex> lock(mutex_);
@@ -232,7 +232,7 @@ void Odometry2DPublisher::notifyCallback(
 void Odometry2DPublisher::onStart()
 {
   synchronizer_ = Synchronizer(device_id_);
-  latest_stamp_ = latest_covariance_stamp_ = Synchronizer::TIME_ZERO;
+  latest_stamp_ = latest_covariance_stamp_ = Synchronizer::ROS_TIME_ZERO;
   latest_covariance_valid_ = false;
   odom_output_ = nav_msgs::Odometry();
   acceleration_output_ = geometry_msgs::AccelWithCovarianceStamped();
@@ -336,7 +336,7 @@ void Odometry2DPublisher::publishTimerCallback()
     acceleration_output = acceleration_output_;
   }
 
-  if (latest_stamp == Synchronizer::TIME_ZERO)
+  if (latest_stamp == Synchronizer::ROS_TIME_ZERO)
   {
     RCLCPP_WARN_STREAM_EXPRESSION(
       node_->get_logger(), delayed_throttle_filter_.isEnabled(),

fuse_models/src/unicycle_2d.cpp

@@ -420,6 +420,7 @@ void Unicycle2D::updateStateHistoryEstimates(
   if (ending_stamp.seconds() > buffer_length.seconds()) {
     auto expiration_time = ending_stamp - buffer_length;
   } else {
+    // NOTE(CH3): Uninitialized. But okay because it's just used for comparison.
     auto expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type);
   }
 

fuse_optimizers/src/batch_optimizer.cpp

@@ -195,7 +195,7 @@ void BatchOptimizer::transactionCallback(
   auto transaction_clock_type = transaction->stamp()->get_clock_type();
 
   rclcpp::Time transaction_time = transaction->stamp();
-  rclcpp::Time last_pending_time(0, 0, transaction_clock_type);
+  rclcpp::Time last_pending_time(0, 0, transaction_clock_type);  // NOTE(CH3): Uninitialized
   if (!started_ || transaction_time >= start_time_)
   {
     std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
@@ -212,12 +212,15 @@ void BatchOptimizer::transactionCallback(
       start_time_ = transaction_time;
     }
     // Purge old transactions from the pending queue
-    rclcpp::Time purge_time(0, 1, transaction_clock_type);  // Initialized
+    rclcpp::Time purge_time(0, 0, transaction_clock_type);  // NOTE(CH3): Uninitialized
     if (started_)
     {
       purge_time = start_time_;
     }
-    else if (rclcpp::Time(0, 1, transaction_clock_type) + params_.transaction_timeout < last_pending_time)  // prevent a bad subtraction
+    // NOTE(CH3): In this case we're okay with uninitialized time since it's just used
+    //            for comparison
+    // prevent a bad subtraction
+    else if (rclcpp::Time(0, 0, transaction_clock_type) + params_.transaction_timeout < last_pending_time)
     {
       purge_time = last_pending_time - params_.transaction_timeout;
     }

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -169,6 +169,7 @@ void FixedLagSmoother::optimizationLoop()
   while (ros::ok() && optimization_running_)
   {
     // Wait for the next signal to start the next optimization cycle
+    // NOTE(CH3): Uninitialized, but it's ok since it's meant to get overwritten.
     auto optimization_deadline = rclcpp::Time(0, 0, RCL_ROS_TIME);
     {
       std::unique_lock<std::mutex> lock(optimization_requested_mutex_);
@@ -442,7 +443,7 @@ bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs:
   }
   started_ = false;
   ignited_ = false;
-  setStartTime(rclcpp::Time(0, 0, RCL_ROS_TIME));  // NOTE(CH3): UNINITIALIZED!
+  setStartTime(rclcpp::Time(0, 1, RCL_ROS_TIME));  // NOTE(CH3): INITIALIZED!
   // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the
   //         pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to
   //         prevent the possibility of deadlocks.
@@ -457,7 +458,7 @@ bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs:
     graph_->clear();
     marginal_transaction_ = fuse_core::Transaction();
     timestamp_tracking_.clear();
-    lag_expiration_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
+    lag_expiration_ = rclcpp::Time(0, 1, RCL_ROS_TIME);  // NOTE(CH3): INITIALIZED!
   }
   // Tell all the plugins to start
   startPlugins();
@@ -528,10 +529,12 @@ void FixedLagSmoother::transactionCallback(
       else
       {
         // And purge out old transactions to limit the pending size while waiting for an ignition sensor
-        auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME);
+        auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME);  // NOTE(CH3): Uninitialized
         auto last_pending_time = pending_transactions_.front().stamp();
 
         // rclcpp::Time doesn't allow negatives
+        // NOTE(CH3): In this case we're okay with uninitialized time since it's just used
+        //            for comparison
         if (rclcpp::Time(0, 0, RCL_ROS_TIME) + params_.transaction_timeout < last_pending_time)
         {
           purge_time = last_pending_time - params_.transaction_timeout;

fuse_optimizers/src/variable_stamp_index.cpp

@@ -58,7 +58,8 @@ rclcpp::Time VariableStampIndex::currentStamp(rcl_clock_t defualt_clock_type) co
   }
   else
   {
-    return rclcpp::Time(0, 0, default_clock_type);
+    // NOTE(CH3): Might be useful to figure out where to place uninitialized checks later on...
+    return rclcpp::Time(0, 0, default_clock_type);  // NOTE(CH3): Signals uninitialized
   }
 }
 

fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h

@@ -68,7 +68,7 @@ class StampedVariableSynchronizer
 {
 public:
   FUSE_SMART_PTR_DEFINITIONS(StampedVariableSynchronizer);
-  static const rclcpp::Time TIME_ZERO;  //!< Constant representing a zero timestamp
+  static const rclcpp::Time ROS_TIME_ZERO;  //!< Constant representing a zero timestamp
 
   /**
    * @brief Construct a synchronizer object
@@ -261,13 +261,23 @@ struct is_variable_in_pack<T, Ts...>
 
 }  // namespace detail
 
+// NOTE(CH3): ROS_TIME_ZERO should be uninitialized, it's used for getting latest time!
 template <typename ...Ts>
-const rclcpp::Time StampedVariableSynchronizer<Ts...>::TIME_ZERO = rclcpp::Time(0, 0);
+const rclcpp::Time StampedVariableSynchronizer<Ts...>::ROS_TIME_ZERO =
+  rclcpp::Time(0, 0, RCL_ROS_TIME);
+
+template <typename ...Ts>
+const rclcpp::Time StampedVariableSynchronizer<Ts...>::SYSTEM_TIME_ZERO =
+  rclcpp::Time(0, 0, RCL_SYSTEM_TIME);
+
+template <typename ...Ts>
+const rclcpp::Time StampedVariableSynchronizer<Ts...>::STEADY_TIME_ZERO =
+  rclcpp::Time(0, 0, RCL_STEADY_TIME);
 
 template <typename ...Ts>
 StampedVariableSynchronizer<Ts...>::StampedVariableSynchronizer(const fuse_core::UUID& device_id) :
   device_id_(device_id),
-  latest_common_stamp_(TIME_ZERO)
+  latest_common_stamp_(ROS_TIME_ZERO)  // NOTE(CH3): Uninitialized, for getting latest
 {
   static_assert(detail::allStampedVariables<Ts...>, "All synchronized types must be derived from both "
                                                     "fuse_core::Variable and fuse_variable::Stamped.");
@@ -280,15 +290,15 @@ rclcpp::Time StampedVariableSynchronizer<Ts...>::findLatestCommonStamp(
   const fuse_core::Graph& graph)
 {
   // Clear the previous stamp if the variable was deleted
-  if (latest_common_stamp_ != TIME_ZERO &&
+  if (latest_common_stamp_ != ROS_TIME_ZERO &&
       !detail::all_variables_exist<Ts...>::value(graph, latest_common_stamp_, device_id_))
   {
-    latest_common_stamp_ = TIME_ZERO;
+    latest_common_stamp_ = ROS_TIME_ZERO;
   }
   // Search the transaction for more recent variables
   updateTime(transaction.addedVariables(), graph);
   // If no common timestamp was found, search the whole graph for the most recent variable set
-  if (latest_common_stamp_ == TIME_ZERO)
+  if (latest_common_stamp_ == ROS_TIME_ZERO)
   {
     updateTime(graph.getVariables(), graph);
   }

fuse_publishers/src/pose_2d_publisher.cpp

@@ -209,7 +209,7 @@ void Pose2DPublisher::notifyCallback(
   fuse_core::Graph::ConstSharedPtr graph)
 {
   auto latest_stamp = synchronizer_->findLatestCommonStamp(*transaction, *graph);
-  if (latest_stamp == Synchronizer::TIME_ZERO)
+  if (latest_stamp == Synchronizer::ROS_TIME_ZERO)
   {
     RCLCPP_WARN_STREAM_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 10.0 * 1000,
@@ -300,7 +300,7 @@ void Pose2DPublisher::tfPublishTimerCallback()
 {
   // The tf_transform_ is updated in a separate thread, so we must guard the read/write operations.
   // Only publish if the tf transform is valid
-  if (tf_transform_.header.stamp != Synchronizer::TIME_ZERO)
+  if (tf_transform_.header.stamp != Synchronizer::ROS_TIME_ZERO)
   {
     // Update the timestamp of the transform so the tf tree will continue to be valid
     tf_transform_.header.stamp = event.current_real;