Skip to content

Commit

Permalink
Sprinkled with healthy dose of Doxygen
Browse files Browse the repository at this point in the history
Also refactored the visualization tool out.

Signed-off-by: Arjo Chakravarty <[email protected]>
  • Loading branch information
arjo129 committed Feb 22, 2023
1 parent 882edbe commit ea93a1b
Show file tree
Hide file tree
Showing 5 changed files with 314 additions and 205 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -73,9 +73,13 @@ class EnvironmentVisualizationTool
this->pcPub.Publish(vec);
}

/// \brief The sample resolution
public: gz::msgs::Vector3d vec;

/// \brief Publisher to publish sample resolution
public: transport::Node::Publisher pcPub;

/// \brief Gz transport node
public: transport::Node node;

/// \brief To synchronize member access.
Expand Down
1 change: 1 addition & 0 deletions src/systems/environment_preload/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
gz_add_system(environment-preload
SOURCES
EnvironmentPreload.cc
VisualizationTool.cc
PUBLIC_LINK_LIBS
gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
gz-common${GZ_COMMON_VER}::io
Expand Down
30 changes: 24 additions & 6 deletions src/systems/environment_preload/EnvironmentPreload.cc
Original file line number Diff line number Diff line change
Expand Up @@ -45,43 +45,58 @@ using Units = msgs::DataLoadPathOptions_DataAngularUnits;
//////////////////////////////////////////////////
class gz::sim::systems::EnvironmentPreloadPrivate
{
/// \brief Is the file loaded
public: bool loaded{false};

/// \brief SDF Description
public: std::shared_ptr<const sdf::Element> sdf;

/// \brief GzTransport node
public: transport::Node node;

/// \brief Data descriptions
public: msgs::DataLoadPathOptions dataDescription;

/// \brief mutex to protect the samples and data description
public: std::mutex mtx;

/// \brief Do we need to reload the system.
public: std::atomic<bool> needsReload{false};

/// \brief Visualization Helper
public: std::unique_ptr<EnvironmentVisualizationTool> visualizationPtr;

/// \brief Are visualizations enabled
public: bool visualize{false};

/// \brief Sample resolutions
public: math::Vector3d samples;

/// \brief Is the file loaded
public: bool fileLoaded{false};

public: bool logError{true};
/// \brief File loading error logger
public: bool logFileLoadError{true};

/// \brief Reference to data
public: std::shared_ptr<components::EnvironmentalData> envData;

//////////////////////////////////////////////////
public: EnvironmentPreloadPrivate() :
visualizationPtr(new EnvironmentVisualizationTool) {};

//////////////////////////////////////////////////
public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg)
{
std::lock_guard<std::mutex> lock(this->mtx);
this->dataDescription = _msg;
this->needsReload = true;
this->logError = true;
this->logFileLoadError = true;
this->visualizationPtr->FileReloaded();
gzdbg << "Loading file " << _msg.path() << "\n";
}

//////////////////////////////////////////////////
public: void OnVisualResChanged(const msgs::Vector3d &_resChanged)
{
std::lock_guard<std::mutex> lock(this->mtx);
Expand All @@ -102,6 +117,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
this->visualizationPtr->resample = true;
}

//////////////////////////////////////////////////
public: void ReadSdf(EntityComponentManager &_ecm)
{
if (!this->sdf->HasElement("data"))
Expand Down Expand Up @@ -202,6 +218,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
needsReload = true;
}

//////////////////////////////////////////////////
public: components::EnvironmentalData::ReferenceUnits ConvertUnits(
const Units &_unit)
{
Expand All @@ -216,6 +233,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate
}
}

//////////////////////////////////////////////////
public: void LoadEnvironment(EntityComponentManager &_ecm)
{
try
Expand All @@ -233,11 +251,11 @@ class gz::sim::systems::EnvironmentPreloadPrivate
std::ifstream dataFile(this->dataDescription.path());
if (!dataFile.is_open())
{
if(logError)
if(logFileLoadError)
{
gzerr << "No environmental data file was found at " <<
this->dataDescription.path() << std::endl;
logError = false;
logFileLoadError = false;
}
return;
}
Expand All @@ -260,11 +278,11 @@ class gz::sim::systems::EnvironmentPreloadPrivate
}
catch (const std::invalid_argument &exc)
{
if(logError)
if(logFileLoadError)
{
gzerr << "Failed to load environment data" << std::endl
<< exc.what() << std::endl;
logError = false;
logFileLoadError = false;
}
}

Expand Down
211 changes: 211 additions & 0 deletions src/systems/environment_preload/VisualizationTool.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,211 @@
/*
* Copyright (C) 2022 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.
*
*/

#include "VisualizationTool.hh"

/////////////////////////////////////////////////
EnvironmentVisualizationTool::EnvironmentVisualizationTool()
{
this->pcPub =
this->node.Advertise<gz::msgs::PointCloudPacked>("/point_cloud");
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::CreatePointCloudTopics(
const std::shared_ptr<components::EnvironmentalData> _data) {
this->pubs.clear();
this->sessions.clear();

for (auto key : _data->frame.Keys())
{
this->pubs.emplace(key, node.Advertise<gz::msgs::Float_V>(key));
gz::msgs::Float_V msg;
this->floatFields.emplace(key, msg);
this->sessions.emplace(key, _data->frame[key].CreateSession());
}
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::FileReloaded()
{
this->finishedTime = false;
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::Step(
const UpdateInfo &_info,
const EntityComponentManager& _ecm,
const std::shared_ptr<components::EnvironmentalData> _data,
double _xSamples, double _ySamples, double _zSamples)
{

auto now = std::chrono::steady_clock::now();
std::chrono::duration<double> dt(now - this->lastTick);

if (this->resample)
{
this->CreatePointCloudTopics(_data);
this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples);
this->resample = false;
this->lastTick = now;
}

for (auto &it : this->sessions)
{
auto res =
_data->frame[it.first].StepTo(it.second,
std::chrono::duration<double>(_info.simTime).count());
if (res.has_value())
{
it.second = res.value();
}
else
{
this->finishedTime = true;
return;
}
}

// Publish at 2 hz for now. In future make reconfigureable.
if (dt.count() > 0.5 && !this->finishedTime)
{
this->Visualize(_data, _xSamples, _ySamples, _zSamples);
this->Publish();
lastTick = now;
}
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::Visualize(
const std::shared_ptr<components::EnvironmentalData> data,
double _xSamples, double _ySamples, double _zSamples)
{

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;
std::size_t idx = 0;
for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++)
{
auto x = lower_bound.X() + x_steps * dx;
for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++)
{
auto y = lower_bound.Y() + y_steps * dy;
for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++)
{
auto z = lower_bound.Z() + z_steps * dz;
auto res = frame.LookUp(session, math::Vector3d(x, y, z));

if (res.has_value())
{
this->floatFields[key].mutable_data()->Set(idx,
static_cast<float>(res.value()));
}
else
{
this->floatFields[key].mutable_data()->Set(idx, std::nanf(""));
}
idx++;
}
}
}
}
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::Publish()
{
pcPub.Publish(this->pcMsg);
for(auto &[key, pub] : this->pubs)
{
pub.Publish(this->floatFields[key]);
}
}

/////////////////////////////////////////////////
void EnvironmentVisualizationTool::ResizeCloud(
const std::shared_ptr<components::EnvironmentalData> _data,
const EntityComponentManager& _ecm,
unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples)
{
assert (pubs.size() > 0);

// Assume all data have same point cloud.
gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true,
{{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}});
auto numberOfPoints = _xSamples * _ySamples * _zSamples;
std::size_t dataSize{
static_cast<std::size_t>(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);

auto step = upper_bound - lower_bound;
auto dx = step.X() / _xSamples;
auto dy = step.Y() / _ySamples;
auto dz = step.Z() / _zSamples;

// Populate point cloud
gz::msgs::PointCloudPackedIterator<float> xIter(pcMsg, "x");
gz::msgs::PointCloudPackedIterator<float> yIter(pcMsg, "y");
gz::msgs::PointCloudPackedIterator<float> zIter(pcMsg, "z");

for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++)
{
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;
for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++)
{
auto z = lower_bound.Z() + z_steps * dz;
auto coords = getGridFieldCoordinates(
_ecm, math::Vector3d{x, y, z},
_data);

if (!coords.has_value())
{
continue;
}

auto pos = coords.value();
*xIter = pos.X();
*yIter = pos.Y();
*zIter = pos.Z();
++xIter;
++yIter;
++zIter;
}
}
}
for (auto key : _data->frame.Keys())
{
this->floatFields[key].mutable_data()->Resize(
numberOfPoints, std::nanf(""));
}
}
Loading

0 comments on commit ea93a1b

Please sign in to comment.