Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix enviroment system loading mechanism #1842

Merged
merged 33 commits into from
Oct 3, 2023
Merged
Show file tree
Hide file tree
Changes from 14 commits
Commits
Show all changes
33 commits
Select commit Hold shift + click to select a range
ee16b8d
Fix enviroment system loading mechanism
arjo129 Dec 20, 2022
2ce77fc
small changes
arjo129 Dec 21, 2022
3829724
Working on porting the visuals
arjo129 Jan 12, 2023
f0d1866
Actually send message for loading from ui to environment preload plugin.
arjo129 Feb 16, 2023
9b684cf
Rewrite EnvironmentVisualization Widget to be simpler.
arjo129 Feb 17, 2023
dd24227
fix crashes.
arjo129 Feb 17, 2023
712ebe0
Get a different :boom:
arjo129 Feb 17, 2023
d722ad8
Works some times.
arjo129 Feb 17, 2023
87586d7
Fixed synchronization issues.
arjo129 Feb 20, 2023
77f5472
No more :boom:s :tada:
arjo129 Feb 21, 2023
882edbe
style
arjo129 Feb 22, 2023
ea93a1b
Sprinkled with healthy dose of Doxygen
arjo129 Feb 22, 2023
4fd5c9e
Style
arjo129 Feb 23, 2023
d7c34c3
More style fixes
arjo129 Feb 23, 2023
7af4af9
Fix Typo with unit map
arjo129 Apr 14, 2023
57b949b
Address PR feedback
arjo129 Apr 14, 2023
afcf5c6
Style fixes
arjo129 Apr 17, 2023
6b56a43
Fix incorrect use of path.
arjo129 Apr 17, 2023
8779e84
Merge branch 'gz-sim7' into arjo/fix/environment_system
mjcarroll Apr 25, 2023
c2708db
Fix example loading issues.
arjo129 Jul 18, 2023
c30639a
Merge branch 'arjo/fix/environment_system' of github.com:gazebosim/gz…
arjo129 Jul 18, 2023
54c42b2
style
arjo129 Jul 18, 2023
fe1bc7c
Update src/systems/environment_preload/VisualizationTool.cc
arjo129 Aug 22, 2023
3cf7896
Adds a warning regarding loading plugins.
arjo129 Aug 24, 2023
36e7cc4
Merge remote-tracking branch 'origin' into arjo/fix/environment_system
arjo129 Aug 24, 2023
f90ef4f
Merge branch 'arjo/fix/environment_system' of github.com:gazebosim/gz…
arjo129 Aug 24, 2023
6b2c398
Automatically loads plugin if missing
arjo129 Aug 25, 2023
85b6eac
Address some feedback I missed
arjo129 Aug 25, 2023
04a3858
Address some feedback
arjo129 Aug 25, 2023
e3ed11a
Fixes issue described by @iche033.
arjo129 Aug 28, 2023
4d0a034
style
arjo129 Aug 28, 2023
0672777
Fixed failing tests
arjo129 Sep 4, 2023
9daebc8
Merge branch 'gz-sim7' into arjo/fix/environment_system
iche033 Sep 5, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 41 additions & 38 deletions src/gui/plugins/environment_loader/EnvironmentLoader.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@

#include <gz/gui/Application.hh>
#include <gz/gui/MainWindow.hh>
#include <gz/sim/components/Environment.hh>
#include <gz/sim/Util.hh>

#include <gz/plugin/Register.hh>
Expand All @@ -33,6 +32,11 @@
#include <gz/common/CSVStreams.hh>
#include <gz/common/DataFrame.hh>

#include <gz/transport/Node.hh>

#include <gz/msgs/data_load_options.pb.h>
#include <gz/msgs/Utility.hh>

using namespace gz;
using namespace sim;

Expand All @@ -42,6 +46,8 @@ namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE
{

using Units = msgs::DataLoadPathOptions_DataAngularUnits;
/// \brief Private data class for EnvironmentLoader
class EnvironmentLoaderPrivate
{
Expand All @@ -65,7 +71,7 @@ class EnvironmentLoaderPrivate
public: int zIndex{-1};

/// \brief Index of data dimension to be used as units.
public: QString unit;
public: QString unit{"radians"};

public: using ReferenceT = math::SphericalCoordinates::CoordinateType;

Expand All @@ -76,12 +82,12 @@ class EnvironmentLoaderPrivate
{QString("ecef"), math::SphericalCoordinates::ECEF}};

/// \brief Map of supported spatial units.
public: const QMap<QString, components::EnvironmentalData::ReferenceUnits>
public: const QMap<QString, Units>
unitMap{
{QString("degree"),
components::EnvironmentalData::ReferenceUnits::DEGREES},
Units::DataLoadPathOptions_DataAngularUnits_DEGREES},
{QString("radians"),
components::EnvironmentalData::ReferenceUnits::RADIANS}
Units::DataLoadPathOptions_DataAngularUnits_DEGREES}
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
};

/// \brief Spatial reference.
Expand All @@ -92,6 +98,12 @@ class EnvironmentLoaderPrivate

/// \brief Whether to attempt an environmental data load.
public: std::atomic<bool> needsLoad{false};

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

/// \brief publisher
public: std::optional<transport::Node::Publisher> pub{std::nullopt};
};
}
}
Expand Down Expand Up @@ -123,46 +135,37 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *)
void EnvironmentLoader::Update(const UpdateInfo &,
EntityComponentManager &_ecm)
{
if (this->dataPtr->needsLoad)
if (!this->dataPtr->pub.has_value())
{
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
this->dataPtr->needsLoad = false;

/// TODO(arjo): Handle the case where we are loading a file in windows.
/// Should SDFormat files support going from windows <=> unix paths?
std::ifstream dataFile(this->dataPtr->dataPath.toStdString());
gzmsg << "Loading environmental data from "
<< this->dataPtr->dataPath.toStdString()
<< std::endl;
try
{
using ComponentDataT = components::EnvironmentalData;
auto data = ComponentDataT::MakeShared(
common::IO<ComponentDataT::FrameT>::ReadFrom(
common::CSVIStreamIterator(dataFile),
common::CSVIStreamIterator(),
this->dataPtr->timeIndex, {
static_cast<size_t>(this->dataPtr->xIndex),
static_cast<size_t>(this->dataPtr->yIndex),
static_cast<size_t>(this->dataPtr->zIndex)}),
this->dataPtr->referenceMap[this->dataPtr->reference],
this->dataPtr->unitMap[this->dataPtr->unit]);

using ComponentT = components::Environment;
_ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)});
}
catch (const std::invalid_argument &exc)
{
gzerr << "Failed to load environmental data" << std::endl
<< exc.what() << std::endl;
}
auto world = worldEntity(_ecm);
auto topic = common::joinPaths(scopedName(world, _ecm), "environment");
this->dataPtr->pub =
{this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
}
}

/////////////////////////////////////////////////
void EnvironmentLoader::ScheduleLoad()
{
this->dataPtr->needsLoad = this->IsConfigured();
if(this->IsConfigured() && this->dataPtr->pub.has_value())
{
msgs::DataLoadPathOptions data;
data.set_path(this->dataPtr->dataPath.toStdString());
data.set_time(
this->dataPtr->dimensionList[this->dataPtr->timeIndex].toStdString());
data.set_x(
this->dataPtr->dimensionList[this->dataPtr->xIndex].toStdString());
data.set_y(
this->dataPtr->dimensionList[this->dataPtr->yIndex].toStdString());
data.set_z(
this->dataPtr->dimensionList[this->dataPtr->zIndex].toStdString());
auto referenceFrame = this->dataPtr->referenceMap[this->dataPtr->reference];

data.set_coordinate_type(msgs::ConvertCoord(referenceFrame));
data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]);

this->dataPtr->pub->Publish(data);
}
}

/////////////////////////////////////////////////
Expand Down
Loading