diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf new file mode 100644 index 0000000000..ce9eccacca --- /dev/null +++ b/examples/worlds/environmental_sensor.sdf @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + environmental_data.csv + + 0 + + + x + y + z + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.05 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1 + 30 + sensors/humidity + + + + + + diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc new file mode 100644 index 0000000000..d6479687f1 --- /dev/null +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -0,0 +1,363 @@ +/* + * 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 "EnvironmentPreload.hh" +#include "VisualizationTool.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/Util.hh" + +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; + + /// \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 needsReload{false}; + + /// \brief Visualization Helper + public: std::unique_ptr visualizationPtr; + + /// \brief Are visualizations enabled + public: bool visualize{false}; + + /// \brief Sample resolutions + public: math::Vector3 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; + } + math::Vector3 converted{ + static_cast(ceil(_resChanged.x())), + static_cast(ceil(_resChanged.y())), + static_cast(ceil(_resChanged.z()))}; + if (this->samples.X() == converted.X() && + this->samples.Y() == converted.Y() && + this->samples.Z() == converted.Z()) + { + // If the sample has not changed return. + // This is because resampling is expensive. + return; + } + this->samples = converted; + this->visualize = true; + this->visualizationPtr->Resample(); + } + + ////////////////////////////////////////////////// + public: void ReadSdf(EntityComponentManager &_ecm) + { + if (!this->sdf->HasElement("data")) + { + gzerr << "No environmental data file was specified" << std::endl; + return; + } + + std::lock_guard lock(mtx); + std::string dataPath = + this->sdf->Get("data"); + + if (common::isRelativePath(dataPath)) + { + 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); + + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_RADIANS); + std::string timeColumnName{"t"}; + bool ignoreTime = false; + std::array spatialColumnNames{"x", "y", "z"}; + sdf::ElementConstPtr elem = + this->sdf->FindElement("dimensions"); + msgs::SphericalCoordinatesType spatialReference = + msgs::SphericalCoordinatesType::GLOBAL; + if (elem) + { + if (elem->HasElement("ignore_time")) + { + ignoreTime = elem->Get("ignore_time"); + } + if (elem->HasElement("time")) + { + timeColumnName = elem->Get("time"); + } + elem = elem->FindElement("space"); + if (elem) + { + if (elem->HasAttribute("reference")) + { + const std::string referenceName = + elem->Get("reference"); + if (referenceName == "global") + { + spatialReference = msgs::SphericalCoordinatesType::GLOBAL; + } + else if (referenceName == "spherical") + { + spatialReference = msgs::SphericalCoordinatesType::SPHERICAL; + if (elem->HasAttribute("units")) + { + std::string unitName = elem->Get("units"); + if (unitName == "degrees") + { + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_DEGREES); + } + else if (unitName != "radians") + { + gzerr << "Unrecognized unit " << unitName << "\n"; + } + } + } + else if (referenceName == "ecef") + { + spatialReference = msgs::SphericalCoordinatesType::ECEF; + } + else + { + gzerr << "Unknown reference '" << referenceName << "'" + << std::endl; + return; + } + } + for (size_t i = 0; i < spatialColumnNames.size(); ++i) + { + if (elem->HasElement(spatialColumnNames[i])) + { + spatialColumnNames[i] = + elem->Get(spatialColumnNames[i]); + } + } + } + } + + 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 + { + 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(), + 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(); + this->fileLoaded = true; + } + catch (const std::invalid_argument &exc) + { + 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(); + this->dataPtr->ReadSdf(_ecm); + } + + if (this->dataPtr->needsReload) + { + this->dataPtr->LoadEnvironment(_ecm); + } + + if (this->dataPtr->visualize) + { + std::lock_guard lock(this->dataPtr->mtx); + const auto &samples = this->dataPtr->samples; + this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, + samples.X(), samples.Y(), samples.Z()); + } +} + +// Register this plugin +GZ_ADD_PLUGIN(EnvironmentPreload, System, + EnvironmentPreload::ISystemConfigure, + EnvironmentPreload::ISystemPreUpdate) +GZ_ADD_PLUGIN_ALIAS(EnvironmentPreload, + "gz::sim::systems::EnvironmentPreload") diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc new file mode 100644 index 0000000000..16013901d3 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -0,0 +1,247 @@ +/* + * 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(); + 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" + <finishedTime = true; + continue; + } + this->sessions.emplace(key, sess); + } +} + +///////////////////////////////////////////////// +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, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) +{ + std::lock_guard lock(this->mutex); + if (this->finishedTime) + { + return; + } + const 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; + } + + if (!_data->staticTime) + { + // Progress session pointers to next time stamp + for (auto &it : this->sessions) + { + 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; + } + } + } + + // 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, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) +{ + for (auto key : _data->frame.Keys()) + { + 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++) + { + const auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) + { + const auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) + { + const auto z = lower_bound.Z() + z_steps * dz; + const 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}}); + 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); + + 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); + + 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"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) + { + const auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) + { + const auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) + { + const auto z = lower_bound.Z() + z_steps * dz; + const auto coords = getGridFieldCoordinates( + _ecm, math::Vector3d{x, y, z}, + _data); + + if (!coords.has_value()) + { + continue; + } + + const 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..2214c086b3 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -0,0 +1,146 @@ +/* + * 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 + private: bool 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 visualized + /// \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 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 + /// \param[in] _data The data to be visualized + /// \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, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publishes a sample of the data + /// \param[in] _data The data to be visualized + /// \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, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _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 visualized + /// \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