diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf index 297f9db5f0..ce9eccacca 100644 --- a/examples/worlds/environmental_sensor.sdf +++ b/examples/worlds/environmental_sensor.sdf @@ -36,6 +36,7 @@ name="gz::sim::systems::EnvironmentPreload"> environmental_data.csv + 0 x diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 3aa0ca4e7a..d6479687f1 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -120,7 +120,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate } this->samples = converted; this->visualize = true; - this->visualizationPtr->resample = true; + this->visualizationPtr->Resample(); } ////////////////////////////////////////////////// @@ -282,7 +282,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); - this->visualizationPtr->resample = true; + this->visualizationPtr->Resample(); this->fileLoaded = true; } catch (const std::invalid_argument &exc) @@ -337,7 +337,7 @@ void EnvironmentPreload::PreUpdate( scopedName(world, _ecm) + "/environment/visualize/res"), &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); - this->dataPtr->visualizationPtr->resample = true; + this->dataPtr->visualizationPtr->Resample(); this->dataPtr->ReadSdf(_ecm); } @@ -349,7 +349,7 @@ void EnvironmentPreload::PreUpdate( if (this->dataPtr->visualize) { std::lock_guard lock(this->dataPtr->mtx); - auto samples = this->dataPtr->samples; + const auto &samples = this->dataPtr->samples; this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, samples.X(), samples.Y(), samples.Z()); } diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc index 6498231541..16013901d3 100644 --- a/src/systems/environment_preload/VisualizationTool.cc +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -37,8 +37,11 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics( this->pubs.emplace(key, node.Advertise(key)); gz::msgs::Float_V msg; this->floatFields.emplace(key, msg); + const double time = std::chrono::duration(_info.simTime).count(); - auto sess = _data->frame[key].CreateSession(time); + const auto sess = _data->staticTime ? + _data->frame[key].CreateSession(0.0) : + _data->frame[key].CreateSession(time); if (!_data->frame[key].IsValid(sess)) { gzerr << key << "data is out of time bounds. Nothing will be published" @@ -53,9 +56,17 @@ void EnvironmentVisualizationTool::CreatePointCloudTopics( ///////////////////////////////////////////////// void EnvironmentVisualizationTool::FileReloaded() { + std::lock_guard lock(this->mutex); this->finishedTime = false; } +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Resample() +{ + std::lock_guard lock(this->mutex); + this->resample = true; +} + ///////////////////////////////////////////////// void EnvironmentVisualizationTool::Step( const UpdateInfo &_info, @@ -63,11 +74,12 @@ void EnvironmentVisualizationTool::Step( const std::shared_ptr &_data, unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { + std::lock_guard lock(this->mutex); if (this->finishedTime) { return; } - auto now = std::chrono::steady_clock::now(); + const auto now = std::chrono::steady_clock::now(); std::chrono::duration dt(now - this->lastTick); if (this->resample) @@ -82,23 +94,25 @@ void EnvironmentVisualizationTool::Step( this->lastTick = now; } - // Progress session pointers to next time stamp - for (auto &it : this->sessions) + if (!_data->staticTime) { - auto res = - _data->frame[it.first].StepTo(it.second, - std::chrono::duration(_info.simTime).count()); - if (res.has_value()) - { - it.second = res.value(); - } - else + // Progress session pointers to next time stamp + for (auto &it : this->sessions) { - gzerr << "Data does not exist beyond this time." - << " Not publishing new environment visualization data." - << std::endl; - this->finishedTime = true; - return; + const auto time = std::chrono::duration(_info.simTime).count(); + const auto res = _data->frame[it.first].StepTo(it.second, time); + if (res.has_value()) + { + it.second = res.value(); + } + else + { + gzerr << "Data does not exist beyond this time (t = " << time << ")." + << " Not publishing new environment visualization data." + << std::endl; + this->finishedTime = true; + return; + } } } @@ -118,24 +132,24 @@ void EnvironmentVisualizationTool::Visualize( { for (auto key : _data->frame.Keys()) { - const auto session = this->sessions[key]; - auto frame = _data->frame[key]; - auto [lower_bound, upper_bound] = frame.Bounds(session); - auto step = upper_bound - lower_bound; - auto dx = step.X() / _xSamples; - auto dy = step.Y() / _ySamples; - auto dz = step.Z() / _zSamples; + const auto &session = this->sessions[key]; + const auto &frame = _data->frame[key]; + const auto [lower_bound, upper_bound] = frame.Bounds(session); + const auto step = upper_bound - lower_bound; + const auto dx = step.X() / _xSamples; + const auto dy = step.Y() / _ySamples; + const auto dz = step.Z() / _zSamples; std::size_t idx = 0; for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++) { - auto x = lower_bound.X() + x_steps * dx; + const auto x = lower_bound.X() + x_steps * dx; for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) { - auto y = lower_bound.Y() + y_steps * dy; + const auto y = lower_bound.Y() + y_steps * dy; for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) { - auto z = lower_bound.Z() + z_steps * dz; - auto res = frame.LookUp(session, math::Vector3d(x, y, z)); + const auto z = lower_bound.Z() + z_steps * dz; + const auto res = frame.LookUp(session, math::Vector3d(x, y, z)); if (res.has_value()) { @@ -176,21 +190,21 @@ void EnvironmentVisualizationTool::ResizeCloud( // Assume all data have same point cloud. gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; + const auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; std::size_t dataSize{ static_cast(numberOfPoints * pcMsg.point_step())}; pcMsg.mutable_data()->resize(dataSize); pcMsg.set_height(1); pcMsg.set_width(numberOfPoints); - auto session = this->sessions[this->pubs.begin()->first]; - auto frame = _data->frame[this->pubs.begin()->first]; - auto [lower_bound, upper_bound] = frame.Bounds(session); + const auto &session = this->sessions[this->pubs.begin()->first]; + const auto &frame = _data->frame[this->pubs.begin()->first]; + const auto [lower_bound, upper_bound] = frame.Bounds(session); - auto step = upper_bound - lower_bound; - auto dx = step.X() / _numXSamples; - auto dy = step.Y() / _numYSamples; - auto dz = step.Z() / _numZSamples; + const auto step = upper_bound - lower_bound; + const auto dx = step.X() / _numXSamples; + const auto dy = step.Y() / _numYSamples; + const auto dz = step.Z() / _numZSamples; // Populate point cloud gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); @@ -199,14 +213,15 @@ void EnvironmentVisualizationTool::ResizeCloud( for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) { - auto x = lower_bound.X() + x_steps * dx; + const auto x = lower_bound.X() + x_steps * dx; for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) { - auto y = lower_bound.Y() + y_steps * dy; + const auto y = lower_bound.Y() + y_steps * dy; for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) { - auto z = lower_bound.Z() + z_steps * dz; - auto coords = getGridFieldCoordinates(_ecm, math::Vector3d{x, y, z}, + const auto z = lower_bound.Z() + z_steps * dz; + const auto coords = getGridFieldCoordinates( + _ecm, math::Vector3d{x, y, z}, _data); if (!coords.has_value()) @@ -214,7 +229,7 @@ void EnvironmentVisualizationTool::ResizeCloud( continue; } - auto pos = coords.value(); + const auto pos = coords.value(); *xIter = pos.X(); *yIter = pos.Y(); *zIter = pos.Z(); diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh index 06505c042b..2214c086b3 100644 --- a/src/systems/environment_preload/VisualizationTool.hh +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -62,7 +62,7 @@ class EnvironmentVisualizationTool private: bool first{true}; /// \brief Enable resampling - public: std::atomic resample{true}; + private: bool resample{true}; /// \brief Time has come to an end. private: bool finishedTime{false}; @@ -78,6 +78,9 @@ class EnvironmentVisualizationTool /// \brief Invoke when new file is made available. public: void FileReloaded(); + /// \brief Invoke when new file is made available. + public: void Resample(); + /// \brief Step the visualizations /// \param[in] _info The simulation info including timestep /// \param[in] _ecm The Entity-Component-Manager