Add API to set next sensor update time (#196)

Signed-off-by: Michael Carroll <michael@openrobotics.org>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>

include/ignition/sensors/Sensor.hh

@@ -99,6 +99,13 @@ namespace ignition
       /// \brief Return the next time the sensor will generate data
       public: std::chrono::steady_clock::duration NextDataUpdateTime() const;
 
+      /// \brief Manually set the next time the sensor will generate data
+      /// Useful for accomodating jumps backwards in time as well
+      /// as specifying updates for non-uniformly updating sensors
+      /// \param[in] _time The next update time
+      public: void SetNextDataUpdateTime(
+                  const std::chrono::steady_clock::duration &_time);
+
       /// \brief Update the sensor.
       ///
       ///   This is called by the manager, and is responsible for determining

src/Sensor.cc

@@ -433,7 +433,14 @@ bool Sensor::Update(const std::chrono::steady_clock::duration &_now,
     // Update the time the plugin should be loaded
     auto delta = std::chrono::duration_cast< std::chrono::milliseconds>
       (std::chrono::duration< double >(1.0 / this->dataPtr->updateRate));
+
     this->dataPtr->nextUpdateTime += delta;
+
+    // Catch up to "now", if necessary.
+    while (this->dataPtr->nextUpdateTime <= _now)
+    {
+      this->dataPtr->nextUpdateTime += delta;
+    }
   }
 
   return result;
@@ -445,6 +452,13 @@ std::chrono::steady_clock::duration Sensor::NextDataUpdateTime() const
   return this->dataPtr->nextUpdateTime;
 }
 
+//////////////////////////////////////////////////
+void Sensor::SetNextDataUpdateTime(
+    const std::chrono::steady_clock::duration &_time)
+{
+  this->dataPtr->nextUpdateTime = _time;
+}
+
 /////////////////////////////////////////////////
 void Sensor::AddSequence(ignition::msgs::Header *_msg,
                          const std::string &_seqKey)

src/Sensor_TEST.cc

@@ -342,6 +342,70 @@ TEST(Sensor_TEST, SetRateZeroService)
   EXPECT_FLOAT_EQ(20.0, sensor.UpdateRate());
 }
 
+//////////////////////////////////////////////////
+TEST_F(SensorUpdate, NextDataUpdateTime)
+{
+  // Create sensor.
+  sdf::Sensor sdfSensor;
+  sdfSensor.SetName(kSensorName);
+  sdfSensor.SetTopic(kSensorTopic);
+  sdfSensor.SetUpdateRate(1);
+
+  std::unique_ptr<Sensor> sensor = std::make_unique<TestSensor>();
+  sensor->Load(sdfSensor);
+
+  {
+    std::chrono::steady_clock::duration now = std::chrono::seconds(0);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(1);
+    EXPECT_TRUE(sensor->Update(now, false));
+    EXPECT_EQ(next.count(), sensor->NextDataUpdateTime().count());
+  }
+
+  {
+    std::chrono::steady_clock::duration now = std::chrono::seconds(1);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(2);
+    EXPECT_TRUE(sensor->Update(now, false));
+    EXPECT_EQ(next.count(), sensor->NextDataUpdateTime().count());
+  }
+
+  {
+    // set the next data update time into the future, so we no longer
+    // expect an update to happen
+    std::chrono::steady_clock::duration now = std::chrono::seconds(2);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(5);
+    sensor->SetNextDataUpdateTime(next);
+    EXPECT_FALSE(sensor->Update(now, false));
+    EXPECT_EQ(next.count(), sensor->NextDataUpdateTime().count());
+  }
+
+  {
+    // Force has no impact  on the next update time
+    std::chrono::steady_clock::duration now = std::chrono::seconds(3);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(5);
+    EXPECT_TRUE(sensor->Update(now, true));
+    EXPECT_EQ(next.count(), sensor->NextDataUpdateTime().count());
+  }
+
+  {
+    // When that time point is reached, the update happens
+    std::chrono::steady_clock::duration now = std::chrono::seconds(5);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(6);
+    EXPECT_TRUE(sensor->Update(now, false));
+    EXPECT_EQ(next.count(), sensor->NextDataUpdateTime().count());
+  }
+
+  {
+    // Jump backwards in time
+    std::chrono::steady_clock::duration now = std::chrono::seconds(5);
+    std::chrono::steady_clock::duration next = std::chrono::seconds(1);
+    sensor->SetNextDataUpdateTime(next);
+    EXPECT_TRUE(sensor->Update(now, false));
+
+    // The next update should be the first dt past the current time
+    std::chrono::steady_clock::duration newNext = std::chrono::seconds(6);
+    EXPECT_EQ(newNext.count(), sensor->NextDataUpdateTime().count());
+  }
+}
 
 //////////////////////////////////////////////////
 int main(int argc, char **argv)