From f6efeefb4ab99284d1a053061235f8c2fa02a5da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 3 Oct 2023 08:54:08 +0800 Subject: [PATCH] Fix enviroment system loading mechanism (#1842) * Fix enviroment system loading mechanism Currently, there is an issue with the way the Environment loader plugin loads data. In particular it directly writes to the ECM. While this makes sense intuitively, it does not work in practice as the GUI runs on a client process while systems that use it run on the server. This PR fixes this issue by introducing a topic through which the GUI may load Environment Data on the server. Signed-off-by: Arjo Chakravarty * small changes Signed-off-by: Arjo Chakravarty * Working on porting the visuals Signed-off-by: Arjo Chakravarty * Actually send message for loading from ui to environment preload plugin. Visuallization still goes :boom: Signed-off-by: Arjo Chakravarty * Rewrite EnvironmentVisualization Widget to be simpler. Signed-off-by: Arjo Chakravarty * fix crashes. Vis still not working Signed-off-by: Arjo Chakravarty * Get a different :boom: Signed-off-by: Arjo Chakravarty * Works some times. Signed-off-by: Arjo Chakravarty * Fixed synchronization issues. Now left with one more crash that needs debugging when "play" is hit. Signed-off-by: Arjo Chakravarty * No more :boom:s :tada: Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Sprinkled with healthy dose of Doxygen Also refactored the visualization tool out. Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty * More style fixes Signed-off-by: Arjo Chakravarty * Fix Typo with unit map Signed-off-by: Arjo Chakravarty * Address PR feedback Signed-off-by: Arjo Chakravarty * Style fixes Signed-off-by: Arjo Chakravarty * Fix incorrect use of path. Signed-off-by: Arjo Chakravarty * Fix example loading issues. Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Update src/systems/environment_preload/VisualizationTool.cc Co-authored-by: Mabel Zhang Signed-off-by: Arjo Chakravarty * Adds a warning regarding loading plugins. Signed-off-by: Arjo Chakravarty * Automatically loads plugin if missing This commit automatically loads the environment preload plugin if it is missing. Signed-off-by: Arjo Chakravarty * Address some feedback I missed Signed-off-by: Arjo Chakravarty * Address some feedback Signed-off-by: Arjo Chakravarty * Fixes issue described by @iche033. However fix depends on https://github.com/gazebosim/gz-math/pull/551 Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Fixed failing tests Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Co-authored-by: Michael Carroll Co-authored-by: Mabel Zhang Co-authored-by: Ian Chen --- examples/worlds/CMakeLists.txt | 4 + examples/worlds/environmental_sensor.sdf | 11 +- .../environment_loader/EnvironmentLoader.cc | 119 +++++--- .../EnvironmentVisualization.cc | 244 +++------------- .../EnvironmentVisualization.hh | 6 +- .../environment_preload/CMakeLists.txt | 1 + .../environment_preload/EnvironmentPreload.cc | 265 ++++++++++++++---- .../environment_preload/VisualizationTool.cc | 232 +++++++++++++++ .../environment_preload/VisualizationTool.hh | 143 ++++++++++ 9 files changed, 732 insertions(+), 293 deletions(-) create mode 100644 src/systems/environment_preload/VisualizationTool.cc create mode 100644 src/systems/environment_preload/VisualizationTool.hh diff --git a/examples/worlds/CMakeLists.txt b/examples/worlds/CMakeLists.txt index fee3a9e41c..258ee1529e 100644 --- a/examples/worlds/CMakeLists.txt +++ b/examples/worlds/CMakeLists.txt @@ -2,4 +2,8 @@ file(GLOB files "*.sdf") install(FILES ${files} DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) +file(GLOB csv_files "*.csv") +install(FILES ${csv_files} + DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) + add_subdirectory(thumbnails) diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf index e1947c8591..8ef8ed35f1 100644 --- a/examples/worlds/environmental_sensor.sdf +++ b/examples/worlds/environmental_sensor.sdf @@ -1,5 +1,12 @@ - + + @@ -104,6 +112,7 @@ 1 30 + sensors/humidity diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index 6ac8b464cf..cb688b3a8a 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -19,10 +19,11 @@ #include #include -#include #include +#include #include +#include #include #include @@ -33,6 +34,11 @@ #include #include +#include + +#include +#include + using namespace gz; using namespace sim; @@ -42,6 +48,11 @@ namespace sim { inline namespace GZ_SIM_VERSION_NAMESPACE { +const char* preload_plugin_name{ + "gz::sim::systems::EnvironmentPreload"}; +const char* preload_plugin_filename{ + "gz-sim-environment-preload-system"}; +using Units = msgs::DataLoadPathOptions_DataAngularUnits; /// \brief Private data class for EnvironmentLoader class EnvironmentLoaderPrivate { @@ -65,7 +76,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; @@ -76,12 +87,12 @@ class EnvironmentLoaderPrivate {QString("ecef"), math::SphericalCoordinates::ECEF}}; /// \brief Map of supported spatial units. - public: const QMap + public: const QMap unitMap{ {QString("degree"), - components::EnvironmentalData::ReferenceUnits::DEGREES}, + Units::DataLoadPathOptions_DataAngularUnits_DEGREES}, {QString("radians"), - components::EnvironmentalData::ReferenceUnits::RADIANS} + Units::DataLoadPathOptions_DataAngularUnits_RADIANS} }; /// \brief Spatial reference. @@ -92,6 +103,12 @@ class EnvironmentLoaderPrivate /// \brief Whether to attempt an environmental data load. public: std::atomic needsLoad{false}; + + /// \brief Gz transport node + public: transport::Node node; + + /// \brief publisher + public: std::optional pub{std::nullopt}; }; } } @@ -123,38 +140,54 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *) void EnvironmentLoader::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - if (this->dataPtr->needsLoad) + auto world = worldEntity(_ecm); + + if (!this->dataPtr->pub.has_value()) { - std::lock_guard 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 + auto topic = transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/" + "environment"); + this->dataPtr->pub = + {this->dataPtr->node.Advertise(topic)}; + } + + static bool warned = false; + if (!this->dataPtr->pub->HasConnections() && !warned) + { + warned = true; + gzwarn << "Could not find a subscriber for the environment. " + << "Attempting to load environmental preload plugin." + << std::endl; + + auto nameComp = _ecm.Component(world); + if (nullptr == nameComp) { + gzerr << "Failed to get world name" << std::endl; + return; + } + auto worldName = nameComp->Data(); + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(world); + auto plugin = req.add_plugins(); + plugin->set_name(preload_plugin_name); + plugin->set_filename(preload_plugin_filename); + plugin->set_innerxml(""); + msgs::Boolean res; + bool result; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } + + if (this->dataPtr->node.Request(service, req, timeout, res, result)) { - using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeShared( - common::IO::ReadFrom( - common::CSVIStreamIterator(dataFile), - common::CSVIStreamIterator(), - this->dataPtr->timeIndex, { - static_cast(this->dataPtr->xIndex), - static_cast(this->dataPtr->yIndex), - static_cast(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)}); + gzdbg << "Added plugin successfully" << std::endl; } - catch (const std::invalid_argument &exc) + else { - gzerr << "Failed to load environmental data" << std::endl - << exc.what() << std::endl; + gzerr << "Failed to load plugin" << std::endl; } } } @@ -162,7 +195,25 @@ void EnvironmentLoader::Update(const UpdateInfo &, ///////////////////////////////////////////////// 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); + } } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index 3ed5f6f9ce..c9c040f454 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -36,9 +36,7 @@ #include -#include -#include -#include +#include #include using namespace gz; @@ -51,201 +49,44 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Private data class for EnvironmentVisualization -class EnvironmentVisualizationPrivate +class EnvironmentVisualizationTool { - public: EnvironmentVisualizationPrivate() - { - this->pcPub = - this->node.Advertise("/point_cloud"); - } - /// \brief To synchronize member access. - public: std::mutex mutex; - - /// \brief first load we need to scan for existing data sensor - public: bool first {true}; - - public: std::atomic resample{true}; ///////////////////////////////////////////////// - public: void CreatePointCloudTopics( - std::shared_ptr data) { - this->pubs.clear(); - this->sessions.clear(); - for (auto key : data->frame.Keys()) - { - this->pubs.emplace(key, node.Advertise(key)); - gz::msgs::Float_V msg; - this->floatFields.emplace(key, msg); - this->sessions.emplace(key, data->frame[key].CreateSession()); - } - } - - ///////////////////////////////////////////////// - public: void Step( - const UpdateInfo &_info, - std::shared_ptr &data, - const EntityComponentManager& _ecm, - double xSamples, double ySamples, double zSamples) + public: void Initiallize( + const EntityComponentManager &_ecm) { - auto now = std::chrono::steady_clock::now(); - std::chrono::duration 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(_info.simTime).count()); - if (res.has_value()) - { - it.second = res.value(); - } - } - - // Publish at 2 hz for now. In future make reconfigureable. - if (dt.count() > 0.5) - { - this->Visualize(data, xSamples, ySamples, zSamples); - this->Publish(); - lastTick = now; - } + auto world = worldEntity(_ecm); + auto topic = + common::joinPaths( + scopedName(world, _ecm), "environment", "visualize", "res"); + std::lock_guard lock(this->mutex); + this->pcPub = node.Advertise(topic); } ///////////////////////////////////////////////// public: void Visualize( - std::shared_ptr 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(res.value())); - } - else - { - this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); - } - idx++; - } - } - } - } - } - - ///////////////////////////////////////////////// - public: void Publish() + double xSamples, double ySamples, double zSamples) { - pcPub.Publish(this->pcMsg); - for(auto &[key, pub] : this->pubs) - { - pub.Publish(this->floatFields[key]); - } + std::lock_guard lock(this->mutex); + this->vec = msgs::Convert(math::Vector3d(xSamples, ySamples, zSamples)); + this->pcPub.Publish(vec); } - ///////////////////////////////////////////////// - public: void ResizeCloud( - std::shared_ptr 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(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); + /// \brief The sample resolution + public: gz::msgs::Vector3d vec; - 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 xIter(pcMsg, "x"); - gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); - gz::msgs::PointCloudPackedIterator 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); + /// \brief Publisher to publish sample resolution + public: transport::Node::Publisher pcPub; - if (!coords.has_value()) - { - continue; - } + /// \brief Gz transport node + public: transport::Node node; - 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("")); - } - } + /// \brief To synchronize member access. + public: std::mutex mutex; - public: transport::Node::Publisher pcPub; - public: std::unordered_map pubs; - public: std::unordered_map floatFields; - public: transport::Node node; - public: gz::msgs::PointCloudPacked pcMsg; - public: std::unordered_map> sessions; - public: std::chrono::time_point lastTick; + /// \brief first load we need to scan for existing data sensor + public: bool first{true}; }; } } @@ -253,10 +94,14 @@ class EnvironmentVisualizationPrivate ///////////////////////////////////////////////// EnvironmentVisualization::EnvironmentVisualization() - : GuiSystem(), dataPtr(new EnvironmentVisualizationPrivate) + : GuiSystem(), dataPtr(new EnvironmentVisualizationTool) { gui::App()->Engine()->rootContext()->setContextProperty( "EnvironmentVisualization", this); + this->qtimer = new QTimer(this); + connect(qtimer, &QTimer::timeout, + this, &EnvironmentVisualization::ResamplePointcloud); + this->qtimer->start(1000); } ///////////////////////////////////////////////// @@ -274,38 +119,23 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *) } ///////////////////////////////////////////////// -void EnvironmentVisualization::Update(const UpdateInfo &_info, +void EnvironmentVisualization::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - _ecm.EachNew( - [this]( - const Entity &/*_entity*/, - const components::Environment* /*environment*/ - ) { - this->dataPtr->resample = true; - return true; - } - ); - - auto environData = - _ecm.Component( - worldEntity(_ecm)); - - if (environData == nullptr) + if (this->dataPtr->first) { - return; + this->dataPtr->Initiallize(_ecm); + this->dataPtr->first = false; + this->ResamplePointcloud(); } - - this->dataPtr->Step( - _info, environData->Data(), _ecm, - this->xSamples, this->ySamples, this->zSamples - ); } ///////////////////////////////////////////////// void EnvironmentVisualization::ResamplePointcloud() { - this->dataPtr->resample = true; + this->dataPtr->Visualize( + this->xSamples, this->ySamples, this->zSamples + ); } diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh index 91cd420eb9..e5a3eb4397 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -30,7 +30,7 @@ namespace sim // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { - class EnvironmentVisualizationPrivate; + class EnvironmentVisualizationTool; /// \class EnvironmentVisualization EnvironmentVisualization.hh /// gz/sim/systems/EnvironmentVisualization.hh @@ -66,13 +66,15 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \internal /// \brief Pointer to private data - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; public: unsigned int xSamples{10}; public: unsigned int ySamples{10}; public: unsigned int zSamples{10}; + + private: QTimer* qtimer; }; } } diff --git a/src/systems/environment_preload/CMakeLists.txt b/src/systems/environment_preload/CMakeLists.txt index 178ec5ec32..ed6a4b8efe 100644 --- a/src/systems/environment_preload/CMakeLists.txt +++ b/src/systems/environment_preload/CMakeLists.txt @@ -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 diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 5c678aa2db..5192fa46df 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -15,8 +15,10 @@ * */ #include "EnvironmentPreload.hh" +#include "VisualizationTool.hh" #include +#include #include #include #include @@ -27,6 +29,11 @@ #include +#include + +#include +#include + #include "gz/sim/components/Environment.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Util.hh" @@ -35,72 +42,114 @@ using namespace gz; using namespace sim; using namespace systems; +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 sdf; -}; -////////////////////////////////////////////////// -EnvironmentPreload::EnvironmentPreload() - : System(), dataPtr(new EnvironmentPreloadPrivate) -{ -} + /// \brief GzTransport node + public: transport::Node node; -////////////////////////////////////////////////// -EnvironmentPreload::~EnvironmentPreload() = default; + /// \brief Data descriptions + public: msgs::DataLoadPathOptions dataDescription; -////////////////////////////////////////////////// -void EnvironmentPreload::Configure( - const Entity &/*_entity*/, - const std::shared_ptr &_sdf, - EntityComponentManager &/*_ecm*/, - EventManager &/*_eventMgr*/) -{ - this->dataPtr->sdf = _sdf; -} + /// \brief mutex to protect the samples and data description + public: std::mutex mtx; -////////////////////////////////////////////////// -void EnvironmentPreload::PreUpdate( - const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &_ecm) -{ - if (!std::exchange(this->dataPtr->loaded, true)) + /// \brief Do we need to reload the system. + public: std::atomic needsReload{false}; + + /// \brief Visualization Helper + public: std::unique_ptr 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}; + + /// \brief File loading error logger + public: bool logFileLoadError{true}; + + /// \brief Reference to data + public: std::shared_ptr envData; + + ////////////////////////////////////////////////// + public: EnvironmentPreloadPrivate() : + visualizationPtr(new EnvironmentVisualizationTool) {} + + ////////////////////////////////////////////////// + public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg) + { + std::lock_guard lock(this->mtx); + this->dataDescription = _msg; + this->needsReload = true; + this->logFileLoadError = true; + this->visualizationPtr->FileReloaded(); + gzdbg << "Loading file " << _msg.path() << "\n"; + } + + ////////////////////////////////////////////////// + public: void OnVisualResChanged(const msgs::Vector3d &_resChanged) + { + std::lock_guard lock(this->mtx); + if (!this->fileLoaded) + { + // Only visualize if a file exists + return; + } + auto converted = msgs::Convert(_resChanged); + if (this->samples == converted) + { + // If the sample has not changed return. + // This is because resampling is expensive. + return; + } + this->samples = converted; + this->visualize = true; + this->visualizationPtr->resample = true; + } + + ////////////////////////////////////////////////// + public: void ReadSdf(EntityComponentManager &_ecm) { - if (!this->dataPtr->sdf->HasElement("data")) + if (!this->sdf->HasElement("data")) { - gzerr << "No environmental data file was specified"; + gzerr << "No environmental data file was specified" << std::endl; return; } + std::lock_guard lock(mtx); std::string dataPath = - this->dataPtr->sdf->Get("data"); + this->sdf->Get("data"); + if (common::isRelativePath(dataPath)) { - auto * component = + auto *component = _ecm.Component(worldEntity(_ecm)); const std::string rootPath = common::parentPath(component->Data().Element()->FilePath()); dataPath = common::joinPaths(rootPath, dataPath); } + this->dataDescription.set_path(dataPath); - std::ifstream dataFile(dataPath); - if (!dataFile.is_open()) - { - gzerr << "No environmental data file was found at " << dataPath; - return; - } - - components::EnvironmentalData::ReferenceUnits unit{ - components::EnvironmentalData::ReferenceUnits::RADIANS}; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_RADIANS); std::string timeColumnName{"t"}; bool ignoreTime = false; std::array spatialColumnNames{"x", "y", "z"}; - auto spatialReference = math::SphericalCoordinates::GLOBAL; sdf::ElementConstPtr elem = - this->dataPtr->sdf->FindElement("dimensions"); + this->sdf->FindElement("dimensions"); + msgs::SphericalCoordinatesType spatialReference = + msgs::SphericalCoordinatesType::GLOBAL; if (elem) { if (elem->HasElement("ignore_time")) @@ -120,27 +169,28 @@ void EnvironmentPreload::PreUpdate( elem->Get("reference"); if (referenceName == "global") { - spatialReference = math::SphericalCoordinates::GLOBAL; + spatialReference = msgs::SphericalCoordinatesType::GLOBAL; } else if (referenceName == "spherical") { - spatialReference = math::SphericalCoordinates::SPHERICAL; + spatialReference = msgs::SphericalCoordinatesType::SPHERICAL; if (elem->HasAttribute("units")) { std::string unitName = elem->Get("units"); if (unitName == "degrees") { - unit = components::EnvironmentalData::ReferenceUnits::DEGREES; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_DEGREES); } else if (unitName != "radians") { - gzerr << "Unrecognized unit " << unitName << "\n"; + ignerr << "Unrecognized unit " << unitName << "\n"; } } } else if (referenceName == "ecef") { - spatialReference = math::SphericalCoordinates::ECEF; + spatialReference = msgs::SphericalCoordinatesType::ECEF; } else { @@ -160,26 +210,143 @@ void EnvironmentPreload::PreUpdate( } } + this->dataDescription.set_static_time(ignoreTime); + this->dataDescription.set_coordinate_type(spatialReference); + this->dataDescription.set_time(timeColumnName); + this->dataDescription.set_x(spatialColumnNames[0]); + this->dataDescription.set_y(spatialColumnNames[1]); + this->dataDescription.set_z(spatialColumnNames[2]); + + this->needsReload = true; + } + + ////////////////////////////////////////////////// + public: components::EnvironmentalData::ReferenceUnits ConvertUnits( + const Units &_unit) + { + switch (_unit) + { + case Units::DataLoadPathOptions_DataAngularUnits_DEGREES: + return components::EnvironmentalData::ReferenceUnits::DEGREES; + case Units::DataLoadPathOptions_DataAngularUnits_RADIANS: + return components::EnvironmentalData::ReferenceUnits::RADIANS; + default: + gzerr << "Invalid unit conversion. Defaulting to radians." << std::endl; + return components::EnvironmentalData::ReferenceUnits::RADIANS; + } + } + + ////////////////////////////////////////////////// + public: void LoadEnvironment(EntityComponentManager &_ecm) + { try { - gzmsg << "Loading Environment Data\n"; + std::lock_guard lock(this->mtx); + std::array spatialColumnNames{ + this->dataDescription.x(), + this->dataDescription.y(), + this->dataDescription.z()}; + + math::SphericalCoordinates::CoordinateType spatialReference = + msgs::Convert(this->dataDescription.coordinate_type()); + auto units = this->ConvertUnits(this->dataDescription.units()); + + std::ifstream dataFile(this->dataDescription.path()); + if (!dataFile.is_open()) + { + if (this->logFileLoadError) + { + gzerr << "No environmental data file was found at " << + this->dataDescription.path() << std::endl; + logFileLoadError = false; + } + return; + } + + gzmsg << "Loading Environment Data " << this->dataDescription.path() << + std::endl; + using ComponentDataT = components::EnvironmentalData; auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), - timeColumnName, spatialColumnNames), - spatialReference, unit, ignoreTime); - + this->dataDescription.time(), spatialColumnNames), + spatialReference, units, this->dataDescription.static_time()); + this->envData = data; using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); + this->visualizationPtr->resample = true; + this->fileLoaded = true; } catch (const std::invalid_argument &exc) { - gzerr << "Failed to load environment data" << std::endl - << exc.what() << std::endl; + if (this->logFileLoadError) + { + gzerr << "Failed to load environment data" << std::endl + << exc.what() << std::endl; + this->logFileLoadError = false; + } } + + this->needsReload = false; + } +}; + +////////////////////////////////////////////////// +EnvironmentPreload::EnvironmentPreload() + : System(), dataPtr(new EnvironmentPreloadPrivate) +{ +} + +////////////////////////////////////////////////// +EnvironmentPreload::~EnvironmentPreload() = default; + +////////////////////////////////////////////////// +void EnvironmentPreload::Configure( + const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->sdf = _sdf; +} + +////////////////////////////////////////////////// +void EnvironmentPreload::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (!std::exchange(this->dataPtr->loaded, true)) + { + auto world = worldEntity(_ecm); + + // See https://github.com/gazebosim/gz-sim/issues/1786 + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment"), + &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get()); + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment/visualize/res"), + &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); + + this->dataPtr->visualizationPtr->resample = true; + this->dataPtr->ReadSdf(_ecm); + } + + if (this->dataPtr->needsReload) + { + this->dataPtr->LoadEnvironment(_ecm); + } + + if (this->dataPtr->visualize) + { + std::lock_guard lock(this->dataPtr->mtx); + 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 new file mode 100644 index 0000000000..75b37b6557 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -0,0 +1,232 @@ +/* + * 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("/point_cloud"); +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info) +{ + this->pubs.clear(); + this->sessions.clear(); + + for (auto key : _data->frame.Keys()) + { + 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); + if (!_data->frame[key].IsValid(sess)) + { + gzerr << key << "data is out of time bounds. Nothing will be published" + <finishedTime = true; + continue; + } + this->sessions.emplace(key, sess); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::FileReloaded() +{ + this->finishedTime = false; +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples) +{ + if (this->finishedTime) + { + return; + } + auto now = std::chrono::steady_clock::now(); + std::chrono::duration dt(now - this->lastTick); + + if (this->resample) + { + this->CreatePointCloudTopics(_data, _info); + if (this->finishedTime) { + this->resample = false; + return; + } + this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples); + this->resample = false; + this->lastTick = now; + } + + // Progress session pointers to next time stamp + for (auto &it : this->sessions) + { + auto res = + _data->frame[it.first].StepTo(it.second, + std::chrono::duration(_info.simTime).count()); + if (res.has_value()) + { + it.second = res.value(); + } + else + { + gzerr << "Data does not exist beyond this time." + << " Not publishing new environment visuallization data." + << std::endl; + 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 &_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(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 &_data, + const EntityComponentManager &_ecm, + unsigned int _numXSamples, + unsigned int _numYSamples, + unsigned int _numZSamples) +{ + 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 = _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); + + auto step = upper_bound - lower_bound; + auto dx = step.X() / _numXSamples; + auto dy = step.Y() / _numYSamples; + auto dz = step.Z() / _numZSamples; + + // Populate point cloud + gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) + { + 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; + 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}, + _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("")); + } +} diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh new file mode 100644 index 0000000000..bb2e2fce1b --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -0,0 +1,143 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ +#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + +/// \brief This class helps handle point cloud visuallizations +/// of environment data. +class EnvironmentVisualizationTool +{ + /// \brief Environment constructor + public: EnvironmentVisualizationTool(); + + /// \brief To synchronize member access. + private: std::mutex mutex; + + /// \brief First load we need to scan for existing data sensor + private: bool first{true}; + + /// \brief Enable resampling + public: std::atomic resample{true}; + + /// \brief Time has come to an end. + private: bool finishedTime{false}; + + /// \brief Create publisher structures whenever a new environment is made + /// available. + /// \param[in] _data Data to be visuallized + /// \param[in] _info simulation info for current time step + private: void CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info); + + /// \brief Invoke when new file is made available. + public: void FileReloaded(); + + /// \brief Step the visualizations + /// \param[in] _info The simulation info including timestep + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + public: void Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Publishes a sample of the data + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void Visualize( + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Get the point cloud data. + private: void Publish(); + + /// \brief Resize the point cloud structure (used to reallocate + /// memory when resolution changes) + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publisher for point clouds + private: transport::Node::Publisher pcPub; + + /// \brief Publishers for data + private: std::unordered_map pubs; + + /// \brief Floating point message buffers + private: std::unordered_map floatFields; + + /// \brief GZ buffers + private: transport::Node node; + + /// \brief Point cloud buffer + private: gz::msgs::PointCloudPacked pcMsg; + + /// \brief Session cursors + private: std::unordered_map> sessions; + + /// \brief Duration from last update + private: std::chrono::time_point lastTick; +}; +} +} +} +#endif