From a78b6c5d816b302c5b0525f90db8f14b0e5f8820 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Jun 2022 13:16:28 +0800 Subject: [PATCH] Adds time interpolation to the Science Sensors (#210) * 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 * Add unit test and fix them :bug:s Signed-off-by: Arjo Chakravarty * Address some PR feedback Signed-off-by: Arjo Chakravarty * ign->gz issues Signed-off-by: Arjo Chakravarty --- .../src/ScienceSensorsSystem.cc | 250 +++++++++++------- .../data/minimal_time_varying.csv | 17 ++ .../test/vehicle/CMakeLists.txt | 1 + .../test/vehicle/test_sensor.cc | 2 +- .../vehicle/test_sensor_timeinterpolation.cc | 171 ++++++++++++ 5 files changed, 339 insertions(+), 102 deletions(-) create mode 100644 lrauv_system_tests/data/minimal_time_varying.csv create mode 100644 lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 6eb86171..0b36dba0 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -24,6 +24,7 @@ #include + #include #include #include @@ -32,7 +33,6 @@ #include #include #include - #include #include #include @@ -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 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 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 @@ -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> &_dataArray, + const double _tol = 1e-10); + /////////////////////////////// // Constants for data manipulation @@ -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 timestamps; @@ -174,6 +193,7 @@ class tethys::ScienceSensorsSystemPrivate /// the visuallization. public: std::vector::Ptr> timeSpaceCoords; + public: std::vector> timeSpaceCoordsLatLon; @@ -303,7 +323,70 @@ ScienceSensorsSystem::ScienceSensorsSystem() } ///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::ReadData( +float ScienceSensorsSystemPrivate::InterpolateInTime( + const gz::math::Vector3d &_point, + const double _simTimeSeconds, + const std::vector> &_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"); @@ -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 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 fieldnames; std::string line, word, temp; @@ -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; } ///////////////////////////////////////////////// @@ -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()); } ///////////////////////////////////////////////// @@ -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 lock(this->dataPtr->dataMutex); this->dataPtr->ReadData(_ecm); + this->dataPtr->newDataAvailable = false; } else { @@ -634,22 +736,23 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info, return; } } + else{ + if (this->dataPtr->newDataAvailable.load()) + { + std::lock_guard lock(this->dataPtr->dataMutex); + auto result = this->dataPtr->ReadData(_ecm); + this->dataPtr->newDataAvailable = !result; + } + } - double simTimeSeconds = std::chrono::duration_cast( + double simTimeSeconds = std::chrono::duration( _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 +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(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( 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( 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( 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 { diff --git a/lrauv_system_tests/data/minimal_time_varying.csv b/lrauv_system_tests/data/minimal_time_varying.csv new file mode 100644 index 00000000..a3c07e8d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lrauv_system_tests/test/vehicle/CMakeLists.txt b/lrauv_system_tests/test/vehicle/CMakeLists.txt index 9d18838d..414e3984 100644 --- a/lrauv_system_tests/test/vehicle/CMakeLists.txt +++ b/lrauv_system_tests/test/vehicle/CMakeLists.txt @@ -27,6 +27,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} diff --git a/lrauv_system_tests/test/vehicle/test_sensor.cc b/lrauv_system_tests/test/vehicle/test_sensor.cc index 7274739f..87088638 100644 --- a/lrauv_system_tests/test/vehicle/test_sensor.cc +++ b/lrauv_system_tests/test/vehicle/test_sensor.cc @@ -99,7 +99,7 @@ void SpawnVehicle( } ////////////////////////////////////////////////// -TEST(SensorTest, Sensor) +TEST(SensorTest, PositionInterpolation) { gz::common::Console::SetVerbosity(4); diff --git a/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc b/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc new file mode 100644 index 00000000..87f6ea4c --- /dev/null +++ b/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "TestConstants.hh" + +using namespace std::chrono_literals; + +std::atomic duration; + +/////////////////////////////////////////////// +void TemperatureVeh1Cb(const gz::msgs::Double &_msg) +{ + auto timeNow = std::chrono::duration(duration.load()).count(); + auto temperature = _msg.data(); + + if (timeNow < 10) + { + // At time 0 according to the csv file the temperature is 5. + // Using linear interpolation the temparature at timeNow will be + // (10 * timeNow + (10-timeNow) * 5) / 10 + EXPECT_NEAR(temperature, (10 * timeNow + (10-timeNow) * 5) / 10, 0.1); + } + else + { + // At time 10 according to the csv file temperature is 10. + // Since theres no more data we will just keep the temperature as is. + EXPECT_NEAR(temperature, 10.0, 0.1); + } +} + +/////////////////////////////////////////////// +void SpawnVehicle( + gz::transport::Node::Publisher &_spawnPub, + const std::string &_modelName, + const double _lat, const double _lon, const double _depth, + const int _acommsAddr) +{ + gz::math::Angle lat1 = IGN_DTOR(_lat); + gz::math::Angle lon1 = IGN_DTOR(_lon); + + lrauv_ignition_plugins::msgs::LRAUVInit spawnMsg; + spawnMsg.mutable_id_()->set_data(_modelName); + spawnMsg.set_initlat_(lat1.Degree()); + spawnMsg.set_initlon_(lon1.Degree()); + spawnMsg.set_initz_(_depth); + spawnMsg.set_acommsaddress_(_acommsAddr); + + _spawnPub.Publish(spawnMsg); +} + +////////////////////////////////////////////////// +TEST(SensorTest, TimeInterpolation) +{ + gz::common::Console::SetVerbosity(4); + + // Setup fixture + auto fixture = std::make_unique( + gz::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "worlds", "empty_environment.sdf")); + unsigned int iterations{0u}; + + bool spawnedAllVehicles = {false}; + static const std::string vehicles[] = + {"vehicle1"}; + + fixture->OnPostUpdate( + [&](const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) + { + auto worldEntity = gz::sim::worldEntity(_ecm); + gz::sim::World world(worldEntity); + int numVehiclesSpawned{0}; + for (auto v1 : vehicles) + { + if (world.ModelByName(_ecm, v1) != gz::sim::kNullEntity) + { + numVehiclesSpawned++; + } + } + + if (numVehiclesSpawned == 1) + { + spawnedAllVehicles = true; + } + iterations++; + duration = _info.simTime; + }); + fixture->Finalize(); + + // Check that vehicles don't exist + fixture->Server()->RunOnce(); + EXPECT_EQ(1, iterations); + + int sleep{0}; + int maxSleep{30}; + + // Change the dataconfig + igndbg << "Switching file" <( + "/world/science_sensor/environment_data_path"); + gz::msgs::StringMsg configMsg; + configMsg.set_data(gz::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "data", "minimal_time_varying.csv")); + for (; !configPub.HasConnections() && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(100ms); + } + configPub.Publish(configMsg); + + // Spawn first vehicle + auto spawnPub = node.Advertise( + "/lrauv/init"); + for (; !spawnPub.HasConnections() && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(100ms); + } + ASSERT_LE(sleep, maxSleep); + + + // On a depth coplanar to a piece of data + SpawnVehicle( + spawnPub, "vehicle1", 0.000005, 0.000005, 5, 0); + node.Subscribe("/model/vehicle1/temperature", &TemperatureVeh1Cb); + + // Check that vehicle was spawned + int expectedIterations = iterations; + for (sleep = 0; !spawnedAllVehicles && sleep < maxSleep; + ++sleep) + { + std::this_thread::sleep_for(100ms); + // Run paused so we avoid the physics moving the vehicles + fixture->Server()->RunOnce(true); + expectedIterations++; + igndbg << "Waiting for vehicles to spawn" << std::endl; + } + EXPECT_TRUE(spawnedAllVehicles); + + fixture->Server()->Run(true, 1000, false); +}