Skip to content

Commit

Permalink
Adds time interpolation to the Science Sensors (#210)
Browse files Browse the repository at this point in the history 
* Adds time interpolation.

This PR moves all the logic for lookup in the science sensor to a separate function `ScienceSensorsSystemPrivate::InterpolateInTime()`. It also adds time based interpolation to the science sensor.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Add unit test and fix them 🐛s

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Address some PR feedback

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* ign->gz issues

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
  • Loading branch information
@arjo129
arjo129 authored Jun 15, 2022
1 parent 7efbba3 commit a78b6c5
Show file tree
Hide file tree
Showing 5 changed files with 339 additions and 102 deletions.
250 changes: 149 additions & 101 deletions lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <gz/msgs/pointcloud_packed.pb.h>


#include <gz/common/Profiler.hh>
#include <gz/common/SystemPaths.hh>
#include <gz/sim/World.hh>
Expand All @@ -32,7 +33,6 @@
#include <gz/msgs/Utility.hh>
#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>

#include <pcl/conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
Expand All @@ -52,9 +52,27 @@ 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 gz::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: std::atomic<bool> newDataAvailable{true};

/// \brief Reads csv file and populate various data fields
/// \param[in] _ecm Immutable reference to the ECM
public: void ReadData(const gz::sim::EntityComponentManager &_ecm);
public: bool ReadData(const gz::sim::EntityComponentManager &_ecm);

//////////////////////////////
// Functions for communication
Expand Down Expand Up @@ -85,6 +103,13 @@ class tethys::ScienceSensorsSystemPrivate
/// \brief Returns a point cloud message populated with the latest sensor data
public: gz::msgs::PointCloudPacked PointCloudMsg();

/// \brief Interpolate in time between two sensor data points
public: float InterpolateInTime(
const gz::math::Vector3d &_point,
const double _simTimeSeconds,
const std::vector<std::vector<float>> &_dataArray,
const double _tol = 1e-10);

///////////////////////////////
// Constants for data manipulation

Expand Down Expand Up @@ -150,17 +175,11 @@ class tethys::ScienceSensorsSystemPrivate
/// only shifted once.
public: bool sphericalCoordinatesInitialized{false};

/// \brief Mutex for writing to world origin association to lat/long
public: std::mutex mtx;

//////////////////////////////////
// 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;
Expand All @@ -174,6 +193,7 @@ class tethys::ScienceSensorsSystemPrivate
/// the visuallization.
public: std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> timeSpaceCoords;


public: std::vector<std::vector<gz::math::Vector3d>>
timeSpaceCoordsLatLon;

Expand Down Expand Up @@ -303,7 +323,70 @@ ScienceSensorsSystem::ScienceSensorsSystem()
}

/////////////////////////////////////////////////
void ScienceSensorsSystemPrivate::ReadData(
float ScienceSensorsSystemPrivate::InterpolateInTime(
const gz::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 gz::sim::EntityComponentManager &_ecm)
{
IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData");
Expand All @@ -312,16 +395,28 @@ 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;

Expand Down Expand Up @@ -509,6 +604,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;
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -552,6 +649,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());
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -622,9 +723,10 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::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
{
Expand All @@ -634,22 +736,23 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info,
return;
}
}
else{
if (this->dataPtr->newDataAvailable.load())
{
std::lock_guard<std::mutex> lock(this->dataPtr->dataMutex);
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();
}
}

Expand Down Expand Up @@ -680,97 +783,42 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info,
auto sphericalDepthCorrected = gz::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())
{
gz::math::Temperature tempC;
tempC.SetCelsius(temp.value());
casted->SetData(tempC);
}
else
{
gz::math::Temperature tempC;
tempC.SetCelsius(std::nanf(""));
casted->SetData(tempC);
}
gz::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())
{
gz::math::Vector3d current(nCurr.value(), eCurr.value(), 0);
casted->SetData(current);
}
else
{
gz::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);


gz::math::Vector3d current(nCurr, eCurr, 0);
casted->SetData(current);
}
else
{
Expand Down
17 changes: 17 additions & 0 deletions lrauv_system_tests/data/minimal_time_varying.csv
View file
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit a78b6c5

Please sign in to comment.