Fix some data races

mimosa/src/imu/manager.cpp

@@ -103,53 +103,61 @@ void Manager::callback(const sensor_msgs::Imu::ConstPtr & msg)
     }
   }
 
-  if (propagate) {
-    gtsam::NavState nav_state;
-    {
-      logger_->trace("Propagating IMU");
-      std::lock_guard<std::mutex> lock(propagation_mutex_);
-      if (propagated_upto_ts_ > it_current->first) {
-        // The current imu measurement is older than the last propagated state
-        logger_->warn(
-          "Current IMU measurement is older than the last propagated state. Current ts: {} "
-          "Propagated ts: {}",
-          it_current->first, propagated_upto_ts_);
-        return;
-      }
-
-      if (propagated_upto_ts_ < it_previous->first) {
-        // Normally we will always be doing 1 step propagation, but if we have missed some imu
-        // measurements, we need to propagate up to the current imu measurement
-        updatePreintegrationTo(
-          propagation_base_state_.ts(), it_previous->first, propagation_base_state_.imuBias(),
-          propagation_preintegrator_);
-        propagated_upto_ts_ = it_previous->first;
-      }
-
-      double dt = it_current->first - propagated_upto_ts_;
-
-      // Update the preintegrator with the previous imu measurement for the dt
-      propagation_preintegrator_->integrateMeasurement(
-        it_previous->second.head<3>(), it_previous->second.tail<3>(), dt);
-      propagated_upto_ts_ = it_current->first;
-
-      // Predict the state
-      nav_state = propagation_preintegrator_->predict(
-        propagation_base_state_.navState(), propagation_base_state_.imuBias(),
-        propagation_base_state_.gravity());
-    }
+  if (!propagate) return;
+
+  gtsam::NavState nav_state;
+  logger_->trace("Propagating IMU");
+  std::lock_guard<std::mutex> lock(propagation_mutex_);
+
+  const double dt = it_current->first - propagated_upto_ts_;
 
-    // Publish this new nav_state
-    // Publish odometry
-    nav_msgs::Odometry odometry;
-    if (pub_odometry_.getNumSubscribers()) {
-      odometry.header.frame_id = config_.map_frame;
-      odometry.child_frame_id = config_.body_frame;
-      odometry.header.stamp.fromSec(ts);
-      convert(nav_state.pose(), odometry.pose.pose);
-      convert(nav_state.velocity(), odometry.twist.twist.linear);
-      pub_odometry_.publish(odometry);
+  if (dt < 0) {
+    // The current imu measurement is older than the last propagated state
+    logger_->warn(
+      "Current IMU measurement is older than the last propagated state. Current ts: {} "
+      "Propagated ts: {}",
+      it_current->first, propagated_upto_ts_);
+    return;
+  } else if (dt == 0) {
+    // This can happen if an IMU measurement shares an exact timestamp with a sensor measurement
+    // that just updated the propagation base state, since they share the same hardware clock.
+    // Nothing to integrate, so just publish the propagated state.
+    logger_->debug(
+      "Current IMU measurement has the same timestamp as the last propagated state. Current ts: "
+      "{} "
+      "Propagated ts: {}",
+      it_current->first, propagated_upto_ts_);
+    nav_state = propagation_base_state_.navState();
+  } else {  // Normal case where we need to integrate the new IMU measurement
+    if (propagated_upto_ts_ < it_previous->first) {
+      // Normally we will always be doing 1 step propagation, but if we have missed some imu
+      // measurements, we need to propagate up to the current imu measurement
+      updatePreintegrationTo(
+        propagation_base_state_.ts(), it_previous->first, propagation_base_state_.imuBias(),
+        propagation_preintegrator_);
+      propagated_upto_ts_ = it_previous->first;
     }
+
+    // Update the preintegrator with the previous imu measurement for the dt
+    propagation_preintegrator_->integrateMeasurement(
+      it_previous->second.head<3>(), it_previous->second.tail<3>(), dt);
+    propagated_upto_ts_ = it_current->first;
+
+    // Predict the state
+    nav_state = propagation_preintegrator_->predict(
+      propagation_base_state_.navState(), propagation_base_state_.imuBias(),
+      propagation_base_state_.gravity());
+  }
+
+  // Publish propagated odometry
+  nav_msgs::Odometry odometry;
+  if (pub_odometry_.getNumSubscribers()) {
+    odometry.header.frame_id = config_.map_frame;
+    odometry.child_frame_id = config_.body_frame;
+    odometry.header.stamp.fromSec(ts);
+    convert(nav_state.pose(), odometry.pose.pose);
+    convert(nav_state.velocity(), odometry.twist.twist.linear);
+    pub_odometry_.publish(odometry);
   }
 }
 
@@ -350,6 +358,8 @@ void Manager::getInterpolatedMeasurements(
 
 size_t Manager::getNumMeasurementsBetween(const double t1, const double t2)
 {
+  std::lock_guard<std::mutex> lock(buffer_mutex_);
+
   const double t_start = std::min(t1, t2);
   const double t_end = std::max(t1, t2);
 
@@ -475,25 +485,12 @@ void Manager::setPropagationBaseState(const State & state)
 {
   logger_->trace("Setting propagation base state");
 
-  // we should propagate it to the latest IMU message in the buffer
-  double latest_ts;
-  {
-    std::lock_guard<std::mutex> lock(buffer_mutex_);
-    latest_ts = buffer_.rbegin()->first;
-  }
-
   std::lock_guard<std::mutex> lock(propagation_mutex_);
   if (state.ts() > propagation_base_state_.ts()) {
     propagation_base_state_ = state;
     propagation_preintegrator_->resetIntegrationAndSetBias(propagation_base_state_.imuBias());
     propagated_upto_ts_ = propagation_base_state_.ts();
   }
-
-  if (latest_ts > propagation_base_state_.ts()) {
-    updatePreintegrationTo(
-      propagation_base_state_.ts(), latest_ts, state.imuBias(), propagation_preintegrator_);
-    propagated_upto_ts_ = latest_ts;
-  }
 }
 
 void declare_config(PreintegrationConfig & config)