Adds time interpolation to the Science Sensors

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc

@@ -30,6 +30,7 @@
 #include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/math/VolumetricGridLookupField.hh>
 #include <ignition/msgs/Utility.hh>
+#include <ignition/msgs/stringmsg.pb.h>
 #include <ignition/plugin/Register.hh>
 #include <ignition/transport/Node.hh>
 
@@ -52,9 +53,28 @@ class tethys::ScienceSensorsSystemPrivate
   //////////////////////////////////
   // Functions for data manipulation
 
+  /// \brief Called when plugin is asked to reload file.
+  /// \param[in] _filepath Path to file to reload.
+  public: void OnReloadData(const ignition::msgs::StringMsg &_filepath)
+  {
+    igndbg << "Reloading file " << _filepath.data() << "\n";
+
+    // Trigger reload and reread data
+    std::lock_guard<std::mutex> lock(this->dataMutex);
+    this->newDataAvailable = true;
+    this->dataPath = _filepath.data();
+  }
+
+  /// \brief mutex for updating data.
+  public: std::mutex dataMutex;
+
+  /// \brief Set to true when there is a new file to be read.
+  public: bool newDataAvailable{true};
+
   /// \brief Reads csv file and populate various data fields
   /// \param[in] _ecm Immutable reference to the ECM
-  public: void ReadData(const ignition::gazebo::EntityComponentManager &_ecm);
+  /// \return True if read was successful, false otherwise.
+  public: bool ReadData(const ignition::gazebo::EntityComponentManager &_ecm);
 
   //////////////////////////////
   // Functions for communication
@@ -85,6 +105,13 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Returns a point cloud message populated with the latest sensor data
   public: ignition::msgs::PointCloudPacked PointCloudMsg();
 
+  /// \brief Interpolate in time between two sensor data points
+  public: float InterpolateInTime(
+    const ignition::math::Vector3d &_point,
+    const double _simTimeSeconds,
+    const std::vector<std::vector<float>> &_dataArray,
+    const double _tol = 1e-10);
+
   ///////////////////////////////
   // Constants for data manipulation
 
@@ -156,11 +183,8 @@ class tethys::ScienceSensorsSystemPrivate
   //////////////////////////////////
   // Variables for data manipulation
 
-  /// \brief Whether using more than one time slices of data
-  public: bool multipleTimeSlices {false};
-
   /// \brief Index of the latest time slice
-  public: int timeIdx {0};
+  public: std::size_t timeIdx {0};
 
   /// \brief Timestamps to index slices of data
   public: std::vector<float> timestamps;
@@ -174,7 +198,7 @@ class tethys::ScienceSensorsSystemPrivate
   /// the visuallization.
   public: std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> timeSpaceCoords;
 
-  public: std::vector<std::vector<ignition::math::Vector3d>> 
+  public: std::vector<std::vector<ignition::math::Vector3d>>
     timeSpaceCoordsLatLon;
 
   /// \brief Spatial index of data.
@@ -303,7 +327,70 @@ ScienceSensorsSystem::ScienceSensorsSystem()
 }
 
 /////////////////////////////////////////////////
-void ScienceSensorsSystemPrivate::ReadData(
+float ScienceSensorsSystemPrivate::InterpolateInTime(
+  const ignition::math::Vector3d &_point,
+  const double _simTimeSeconds,
+  const std::vector<std::vector<float>> &_dataArray,
+  const double _tol)
+{
+  // Get spatial interpolators for current time
+  const auto& timeslice1 = this->timeSpaceIndex[this->timeIdx];
+  auto interpolatorsTime1 = timeslice1.GetInterpolators(_point);
+
+  if (interpolatorsTime1.size() == 0) return std::nanf("");
+  if (!interpolatorsTime1[0].index.has_value()) return std::nanf("");
+
+  const auto data1 = timeslice1.EstimateValueUsingTrilinear(
+    interpolatorsTime1,
+    _point,
+    _dataArray[this->timeIdx]
+  );
+
+  if (this->timeIdx + 1 >= this->timeSpaceIndex.size())
+  {
+    // If we reached the end of the dataset then return the last value
+    return data1.value_or(std::nanf(""));
+  }
+
+  // Get spatial interpolators for the next time
+  auto nextTimeIdx = this->timeIdx + 1;
+  const auto& timeslice2 = this->timeSpaceIndex[nextTimeIdx];
+  auto interpolatorsTime2 = timeslice2.GetInterpolators(_point);
+
+  if (interpolatorsTime2.size() == 0) return std::nanf("");
+  if (!interpolatorsTime2[0].index.has_value()) return std::nanf("");
+
+  const auto data2 = timeslice2.EstimateValueUsingTrilinear(
+    interpolatorsTime2,
+    _point,
+    _dataArray[nextTimeIdx]
+  );
+
+
+  auto prevTimeStamp = this->timestamps[this->timeIdx];
+  auto nextTimeStamp = this->timestamps[nextTimeIdx];
+
+  auto dist = nextTimeStamp - prevTimeStamp;
+  if (dist < _tol)
+  {
+    return data1.value_or(std::nanf(""));
+  }
+  else
+  {
+    if (data1.has_value() && data2.has_value())
+    {
+      return (data1.value() * (nextTimeStamp - _simTimeSeconds) +
+        data2.value() * (_simTimeSeconds - prevTimeStamp)) / dist;
+    }
+    else
+    {
+      return std::nanf("");
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+bool ScienceSensorsSystemPrivate::ReadData(
     const ignition::gazebo::EntityComponentManager &_ecm)
 {
   IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData");
@@ -312,16 +399,32 @@ void ScienceSensorsSystemPrivate::ReadData(
   {
     ignerr << "Trying to read data before spherical coordinates were "
            << "initialized." << std::endl;
-    return;
+    return false;
   }
 
   // Lock modifications to world origin spherical association until finish
   // reading and transforming data
   std::lock_guard<std::mutex> lock(mtx);
 
+  // Reset all data
+  timeSpaceCoords.clear();
+  timeSpaceCoordsLatLon.clear();
+  timeSpaceIndex.clear();
+  temperatureArr.clear();
+  salinityArr.clear();
+  chlorophyllArr.clear();
+  eastCurrentArr.clear();
+  northCurrentArr.clear();
+
   std::fstream fs;
   fs.open(this->dataPath, std::ios::in);
 
+  if (!fs.is_open())
+  {
+    ignerr << "Failed to open file [" << this->dataPath << "]" << std::endl;
+    return false;
+  }
+
   std::vector<std::string> fieldnames;
   std::string line, word, temp;
 
@@ -509,6 +612,8 @@ void ScienceSensorsSystemPrivate::ReadData(
   // Make sure the number of timestamps in the 1D indexing array, and the
   // number of time slices of data, are the same.
   assert(this->timestamps.size() == this->timeSpaceCoords.size());
+
+  return true;
 }
 
 /////////////////////////////////////////////////
@@ -552,6 +657,10 @@ void ScienceSensorsSystem::Configure(
     ignmsg << "Loading science data from [" << this->dataPtr->dataPath << "]"
            << std::endl;
   }
+
+  this->dataPtr->node.Subscribe("/world/science_sensor/environment_data_path",
+                                &ScienceSensorsSystemPrivate::OnReloadData,
+                                this->dataPtr.get());
 }
 
 /////////////////////////////////////////////////
@@ -622,9 +731,10 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
     if (this->dataPtr->world.SphericalCoordinates(_ecm))
     {
       this->dataPtr->sphericalCoordinatesInitialized = true;
-
       this->dataPtr->StartTransport();
+      std::lock_guard<std::mutex> lock(this->dataPtr->dataMutex);
       this->dataPtr->ReadData(_ecm);
+      this->dataPtr->newDataAvailable = false;
     }
     else
     {
@@ -634,22 +744,23 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       return;
     }
   }
+  else{
+    std::lock_guard<std::mutex> lock(this->dataPtr->dataMutex);
+    if (this->dataPtr->newDataAvailable)
+    {
+      auto result = this->dataPtr->ReadData(_ecm);
+      this->dataPtr->newDataAvailable = !result;
+    }
+  }
 
-  double simTimeSeconds = std::chrono::duration_cast<std::chrono::seconds>(
+  double simTimeSeconds = std::chrono::duration<double>(
     _info.simTime).count();
 
-  // Update time index
-  if (this->dataPtr->multipleTimeSlices)
+  if(this->dataPtr->timeIdx + 1 < this->dataPtr->timestamps.size())
   {
-    // Only update if sim time exceeds the elapsed timestamp in data
-    if (!this->dataPtr->timestamps.empty() &&
-      simTimeSeconds >= this->dataPtr->timestamps[this->dataPtr->timeIdx])
+    if(simTimeSeconds >= this->dataPtr->timestamps[this->dataPtr->timeIdx + 1])
     {
-      // Increment for next point in time
       this->dataPtr->timeIdx++;
-
-      // Publish science data at the next timestamp
-      this->dataPtr->PublishData();
     }
   }
 
@@ -680,97 +791,42 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
     auto sphericalDepthCorrected = ignition::math::Vector3d{spherical.X(), spherical.Y(),
       -spherical.Z()};
 
-    const auto& timeslice = this->dataPtr->timeSpaceIndex[this->dataPtr->timeIdx];
-    auto interpolators = timeslice.GetInterpolators(sphericalDepthCorrected);
-
-    IGN_PROFILE("ScienceSensorsSystem::Interpolation");
-    if (interpolators.size() == 0) return;
-    if (!interpolators[0].index.has_value()) return;
-
     if (auto casted = std::dynamic_pointer_cast<SalinitySensor>(sensor))
     {
-      const auto sal = timeslice.EstimateValueUsingTrilinear(
-        interpolators,
-        sphericalDepthCorrected,
-        this->dataPtr->salinityArr[this->dataPtr->timeIdx]
-      );
-
-      if (sal.has_value())
-        casted->SetData(sal.value());
-      else
-        casted->SetData(std::nanf(""));
+      casted->SetData(
+        this->dataPtr->InterpolateInTime(
+          sphericalDepthCorrected, simTimeSeconds, this->dataPtr->salinityArr));
     }
     else if (auto casted = std::dynamic_pointer_cast<TemperatureSensor>(
       sensor))
     {
-      const auto temp = timeslice.EstimateValueUsingTrilinear(
-          interpolators,
-          sphericalDepthCorrected,
-          this->dataPtr->temperatureArr[this->dataPtr->timeIdx]
-        );
+      const auto temp = this->dataPtr->InterpolateInTime(
+        sphericalDepthCorrected, simTimeSeconds, this->dataPtr->temperatureArr);
 
-      if (temp.has_value())
-      {
-        ignition::math::Temperature tempC;
-        tempC.SetCelsius(temp.value());
-        casted->SetData(tempC);
-      }
-      else
-      {
-        ignition::math::Temperature tempC;
-        tempC.SetCelsius(std::nanf(""));
-        casted->SetData(tempC);
-      }
+      ignition::math::Temperature tempC;
+      tempC.SetCelsius(temp);
+      casted->SetData(tempC);
     }
     else if (auto casted = std::dynamic_pointer_cast<ChlorophyllSensor>(
       sensor))
     {
-      /// Uncomment to debug
-      /// igndbg << "------------------" << std::endl;
-      /// igndbg << "Sensor position: " << sphericalDepthCorrected << std::endl;
-      /// igndbg << "Got" << interpolators.size() << " interpolators" << std::endl;
-      /// for (auto interp: interpolators)
-      /// {
-      ///   if (!interp.index.has_value()) continue;
-      ///   igndbg << "Chlorophyll sensor: " <<
-      ///     this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx][
-      ///       interp.index.value()] << "@" << interp.position << std::endl;
-      ///   igndbg << "My distance is " << interp.position.X() - sphericalDepthCorrected.X()
-      ///   << ", " << interp.position.Y() - sphericalDepthCorrected.Y() << std::endl;
-      /// }
-      const auto chlor = timeslice.EstimateValueUsingTrilinear(
-          interpolators,
-          sphericalDepthCorrected,
-          this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx]
-        );
-      if (chlor.has_value())
-        casted->SetData(chlor.value());
-      else
-        casted->SetData(std::nanf(""));
+      casted->SetData(
+        this->dataPtr->InterpolateInTime(
+          sphericalDepthCorrected, simTimeSeconds,
+          this->dataPtr->chlorophyllArr));
     }
     else if (auto casted = std::dynamic_pointer_cast<CurrentSensor>(
       sensor))
     {
-      const auto nCurr = timeslice.EstimateValueUsingTrilinear(
-          interpolators,
-          sphericalDepthCorrected,
-          this->dataPtr->northCurrentArr[this->dataPtr->timeIdx]
-        );
-      const auto eCurr = timeslice.EstimateValueUsingTrilinear(
-          interpolators,
-          sphericalDepthCorrected,
-          this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx]
-        );
-      if (nCurr.has_value() && eCurr.has_value())
-      {
-        ignition::math::Vector3d current(nCurr.value(), eCurr.value(), 0);
-        casted->SetData(current);
-      }
-      else
-      {
-        ignition::math::Vector3d current(std::nan(""), std::nan(""), 0);
-        casted->SetData(current);
-      }
+      const auto nCurr = this->dataPtr->InterpolateInTime(
+        sphericalDepthCorrected, simTimeSeconds, this->dataPtr->northCurrentArr);
+
+      const auto eCurr = this->dataPtr->InterpolateInTime(
+        sphericalDepthCorrected, simTimeSeconds, this->dataPtr->eastCurrentArr);
+
+
+      ignition::math::Vector3d current(nCurr, eCurr, 0);
+      casted->SetData(current);
     }
     else
     {

lrauv_system_tests/data/minimal_time_varying.csv

@@ -0,0 +1,17 @@
+elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec
+0,0.00001,0.00000,0,5.0,0.001,0,-1,0.5
+0,0.00001,0.00000,10,5.0,0.001,0,-1,0.5
+0,0.00001,0.00001,0,5.0,0.001,0,-1,0.5
+0,0.00001,0.00001,10,5.0,0.001,0,-1,0.5
+0,0.00000,0.00000,0,5.0,0.001,0,-1,0.5
+0,0.00000,0.00000,10,5.0,0.001,0,-1,0.5
+0,0.00000,0.00001,0,5.0,0.001,0,-1,0.5
+0,0.00000,0.00001,10,5.0,0.001,0,-1,0.5
+10,0.00001,0.00000,0,10.0,0.001,0,-1,0.5
+10,0.00001,0.00000,10,10.0,0.001,0,-1,0.5
+10,0.00001,0.00001,0,10.0,0.001,0,-1,0.5
+10,0.00001,0.00001,10,10.0,0.001,0,-1,0.5
+10,0.00000,0.00000,00,10.0,0.001,0,-1,0.5
+10,0.00000,0.00000,10,10.0,0.001,0,-1,0.5
+10,0.00000,0.00001,0,10.0,0.001,0,-1,0.5
+10,0.00000,0.00001,10,10.0,0.001,0,-1,0.5

lrauv_system_tests/test/vehicle/CMakeLists.txt

@@ -24,6 +24,7 @@ foreach(_test
     test_mass_shifter
     test_propeller_action
     test_rudder_action
+    test_sensor_timeinterpolation
     test_sensor)
   add_executable(${_test} ${_test}.cc)
   target_link_libraries(${_test}