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