diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 734a7e1436..75a58de7a3 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -24,12 +24,15 @@ #include #include + +#include "gz/sim/components/Environment.hh" #include "gz/sim/config.hh" #include "gz/sim/Entity.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Export.hh" #include "gz/sim/Types.hh" + namespace gz { namespace sim @@ -288,6 +291,16 @@ namespace gz std::optional GZ_SIM_VISIBLE sphericalCoordinates( Entity _entity, const EntityComponentManager &_ecm); + /// \brief Get grid field coordinates based on a world position in cartesian + /// coordinate frames. + /// \param[in] _ecm - The Entity Component Manager + /// \param[in] _worldPosition - The world position + /// \param[in] _gridField - The gridfield you are interested in. + std::optional GetGridFieldCoordinates( + const EntityComponentManager &_ecm, + const math::Vector3d& _worldPosition, + const std::shared_ptr& _gridField); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; diff --git a/src/Util.cc b/src/Util.cc index 56f0fca50f..8925e82e16 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -30,6 +30,7 @@ #include "gz/sim/components/Actor.hh" #include "gz/sim/components/Collision.hh" +#include "gz/sim/components/Environment.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/Light.hh" #include "gz/sim/components/Link.hh" @@ -671,6 +672,43 @@ std::optional sphericalCoordinates(Entity _entity, return math::Vector3d(GZ_RTOD(rad.X()), GZ_RTOD(rad.Y()), rad.Z()); } +////////////////////////////////////////////////// +std::optional GetGridFieldCoordinates( + const EntityComponentManager &_ecm, + const math::Vector3d& _worldPosition, + const std::shared_ptr& _gridField + ) +{ + if (_gridField->reference == math::SphericalCoordinates::SPHERICAL) + { + auto origin = + _ecm.Component(worldEntity(_ecm)); + if (!origin) + { + gzerr << "World has no spherical coordinates," + <<" but data was loaded with spherical reference plane" + << std::endl; + return std::nullopt; + } + auto position = origin->Data().PositionTransform( + _worldPosition, math::SphericalCoordinates::LOCAL2, + _gridField->reference); + if (_gridField->units == + components::EnvironmentalData::ReferenceUnits::DEGREES) + { + position = math::Vector3d{ + GZ_RTOD(position.X()), + GZ_RTOD(position.Y()), + position.Z() + }; + } + } + else + { + return _worldPosition; + } +} + ////////////////////////////////////////////////// // Getting the first .sdf file in the path std::string findFuelResourceSdf(const std::string &_path) diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index 743bc7520a..3bcd3a2e54 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -33,6 +33,13 @@ #include #include +#include + +#include +#include +#include +#include + using namespace gz; using namespace sim; @@ -45,17 +52,203 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Private data class for EnvironmentVisualization class EnvironmentVisualizationPrivate { + + public: EnvironmentVisualizationPrivate() + { + this->pcPub = node.Advertise("/point_cloud"); + } /// \brief To synchronize member access. public: std::mutex mutex; - /// \brief Whether to attempt an environmental data load. - public: std::atomic needsLoad{false}; + /// \brief first load we need to scan for existing data sensor + public: bool first {true}; - /// \brief Setup publishers - public: + public: std::atomic resample{true}; - /// \brief first load we need to scan for existing data sensor - public: bool first; + ///////////////////////////////////////////////// + public: void CreatePointCloudTopics( + std::shared_ptr data) { + this->pubs.clear(); + auto keys = data->frame.Keys(); + for (auto key: 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 xResolution, double yResolution, double zResolution) + { + auto now = std::chrono::steady_clock::now(); + std::chrono::duration dt(now - this->lastTick); + + if (this->resample.load()) + { + this->CreatePointCloudTopics(data); + this->ResizeCloud(data, _ecm, xResolution, yResolution, zResolution); + 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) + { + gzerr << "Visualize" << "\n"; + this->Visualize(data, xResolution, yResolution, zResolution); + gzerr << "Publish" << "\n"; + this->Publish(); + lastTick = now; + } + } + + ///////////////////////////////////////////////// + public: void Visualize( + std::shared_ptr data, + double xResolution, double yResolution, double zResolution) { + + 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() / xResolution; + auto dy = step.Y() / yResolution; + auto dz = step.Z() / zResolution; + std::size_t idx = 0; + for (std::size_t x_steps = 0; x_steps < ceil(xResolution); x_steps++) + { + for (std::size_t y_steps = 0; y_steps < ceil(yResolution); y_steps++) + { + for (std::size_t z_steps = 0; z_steps < ceil(zResolution); z_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + auto y = lower_bound.Y() + y_steps * dy; + 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())); + } + idx++; + } + } + } + } + } + + ///////////////////////////////////////////////// + public: void Publish() + { + pcPub.Publish(this->pcMsg); + for(auto &[key, pub]: this->pubs) + { + pub.Publish(this->floatFields[key]); + } + } + + ///////////////////////////////////////////////// + public: void ResizeCloud( + std::shared_ptr data, + const EntityComponentManager& _ecm, + double xResolution, double yResolution, double zResolution) + { + 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 = + ceil(xResolution) * ceil(yResolution) * ceil(zResolution); + unsigned int dataSize{numberOfPoints * pcMsg.point_step()}; + gzerr << "Datasize " << dataSize << "for" << numberOfPoints << "\n"; + + pcMsg.mutable_data()->resize(dataSize+1); + 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() / xResolution; + auto dy = step.Y() / yResolution; + auto dz = step.Z() / zResolution; + + // 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 < ceil(xResolution); x_steps++) + { + for (std::size_t y_steps = 0; y_steps < ceil(yResolution); y_steps++) + { + for (std::size_t z_steps = 0; z_steps < ceil(zResolution); z_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + auto y = lower_bound.Y() + y_steps * dy; + auto z = lower_bound.Z() + z_steps * dz; + + auto coords = GetGridFieldCoordinates( + _ecm, math::Vector3d{x, y, z}, + data); + + if (!coords.has_value()) + { + return; + } + + 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("")); + } + } + + 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; }; } } @@ -84,22 +277,51 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *) } ///////////////////////////////////////////////// -void EnvironmentVisualization::Update(const UpdateInfo &, +void EnvironmentVisualization::Update(const UpdateInfo &_info, 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) + { + return; + } + + this->dataPtr->Step( + _info, environData->Data(), _ecm, + this->xResolution, this->yResolution, this->zResolution + ); } +///////////////////////////////////////////////// +/*void EnvironmentVisualization::xResolutionChanged() +{ + this->dataPtr->resample = true; +} + +///////////////////////////////////////////////// +void EnvironmentVisualization::yResolutionChanged() +{ + this->dataPtr->resample = true; +} + +///////////////////////////////////////////////// +void EnvironmentVisualization::zResolutionChanged() +{ + this->dataPtr->resample = true; +}*/ + // Register this plugin GZ_ADD_PLUGIN(gz::sim::EnvironmentVisualization, gz::gui::Plugin) diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh index 43f163ac40..28b1780639 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -40,6 +40,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { Q_OBJECT + Q_PROPERTY(qreal xResolution MEMBER xResolution NOTIFY xResolutionChanged); + Q_PROPERTY(qreal yResolution MEMBER yResolution NOTIFY yResolutionChanged); + Q_PROPERTY(qreal zResolution MEMBER zResolution NOTIFY zResolutionChanged); + /// \brief Constructor public: EnvironmentVisualization(); @@ -53,9 +57,21 @@ inline namespace GZ_SIM_VERSION_NAMESPACE public: void Update(const UpdateInfo &, EntityComponentManager &_ecm) override; + signals: void xResolutionChanged(); + + signals: void yResolutionChanged(); + + signals: void zResolutionChanged(); + /// \internal /// \brief Pointer to private data private: std::unique_ptr dataPtr; + + public: qreal xResolution{10}; + + public: qreal yResolution{10}; + + public: qreal zResolution{10}; }; } } diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml index 8231e2c2a8..496e731f18 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml @@ -55,8 +55,11 @@ GridLayout { Layout.columnSpan: 6 id: stepSliderX from: 10 - value: 2 + value: 10 to: 1000 + onMoved: function() { + EnvironmentVisualization.xResolution = value; + } } Label { @@ -74,8 +77,11 @@ GridLayout { Layout.columnSpan: 6 id: stepSliderY from: 10 - value: 2 + value: 10 to: 1000 + onMoved: function() { + EnvironmentVisualization.yResolution = value; + } } Label { @@ -93,7 +99,10 @@ GridLayout { Layout.columnSpan: 6 id: stepSliderZ from: 10 - value: 2 + value: 10 to: 1000 + onMoved: function() { + EnvironmentVisualization.zResolution = value; + } } }