From 772409e62321bd2a9e9ae3634dd1228e83056d61 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 16 Nov 2022 08:24:02 +0800 Subject: [PATCH 01/11] Backport #1748: Adds a tool for environment data visualization and custom environmental sensors This PR adds a tool to visualize Scalar Environmental Data. It also adds custom sensors. Signed-off-by: Arjo Chakravarty --- examples/worlds/environmental_data.csv | 17 + examples/worlds/environmental_sensor.sdf | 112 ++++++ include/gz/sim/Util.hh | 14 + include/gz/sim/components/Environment.hh | 16 +- src/Util.cc | 40 ++ src/components/Environment.cc | 5 +- src/gui/plugins/CMakeLists.txt | 1 + .../environment_loader/EnvironmentLoader.cc | 38 +- .../environment_loader/EnvironmentLoader.hh | 26 ++ .../environment_loader/EnvironmentLoader.qml | 29 +- .../environment_visualization/CMakeLists.txt | 8 + .../EnvironmentVisualization.cc | 313 +++++++++++++++ .../EnvironmentVisualization.hh | 80 ++++ .../EnvironmentVisualization.qml | 111 ++++++ .../EnvironmentVisualization.qrc | 5 + src/systems/CMakeLists.txt | 1 + .../environment_preload/EnvironmentPreload.cc | 23 +- .../CMakeLists.txt | 8 + .../EnvironmentalSensorSystem.cc | 359 ++++++++++++++++++ .../EnvironmentalSensorSystem.hh | 68 ++++ test/integration/CMakeLists.txt | 1 + .../integration/environment_preload_system.cc | 63 ++- .../environmental_sensor_system.cc | 83 ++++ test/worlds/CMakeLists.txt | 2 + test/worlds/environmental_data.sdf.in | 13 +- test/worlds/environmental_sensor.csv | 17 + test/worlds/environmental_sensor.sdf.in | 112 ++++++ 27 files changed, 1554 insertions(+), 11 deletions(-) create mode 100644 examples/worlds/environmental_data.csv create mode 100644 examples/worlds/environmental_sensor.sdf create mode 100644 src/gui/plugins/environment_visualization/CMakeLists.txt create mode 100644 src/gui/plugins/environment_visualization/EnvironmentVisualization.cc create mode 100644 src/gui/plugins/environment_visualization/EnvironmentVisualization.hh create mode 100644 src/gui/plugins/environment_visualization/EnvironmentVisualization.qml create mode 100644 src/gui/plugins/environment_visualization/EnvironmentVisualization.qrc create mode 100644 src/systems/environmental_sensor_system/CMakeLists.txt create mode 100644 src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc create mode 100644 src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh create mode 100644 test/integration/environmental_sensor_system.cc create mode 100644 test/worlds/environmental_sensor.csv create mode 100644 test/worlds/environmental_sensor.sdf.in diff --git a/examples/worlds/environmental_data.csv b/examples/worlds/environmental_data.csv new file mode 100644 index 0000000000..fbd1ee7d0a --- /dev/null +++ b/examples/worlds/environmental_data.csv @@ -0,0 +1,17 @@ +timestamp,humidity,x,y,z +0,80,-1,-1,-1 +0,80,-1,-1, 1 +0,80,-1, 1,-1 +0,80,-1, 1, 1 +0,90, 1,-1,-1 +0,90, 1,-1, 1 +0,90, 1, 1,-1 +0,90, 1, 1, 1 +1,90,-1,-1,-1 +1,90,-1,-1, 1 +1,90,-1, 1,-1 +1,90,-1, 1, 1 +1,90, 1,-1,-1 +1,90, 1,-1, 1 +1,90, 1, 1,-1 +1,90, 1, 1, 1 diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf new file mode 100644 index 0000000000..e1947c8591 --- /dev/null +++ b/examples/worlds/environmental_sensor.sdf @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + environmental_data.csv + + + + 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 + + + + + + diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 734a7e1436..11e9da8db3 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -19,17 +19,21 @@ #include +#include #include #include #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 +292,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 Entity Component Manager + /// \param[in] _worldPosition world position + /// \param[in] _gridField 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/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh index a0e721e8cd..57c84a4cf0 100644 --- a/include/gz/sim/components/Environment.hh +++ b/include/gz/sim/components/Environment.hh @@ -46,6 +46,12 @@ namespace components using FrameT = common::DataFrame; using ReferenceT = math::SphericalCoordinates::CoordinateType; + /// \brief Reference units + enum class ReferenceUnits { + RADIANS = 0, + DEGREES + }; + /// \brief Instantiate environmental data. /// /// An std::make_shared equivalent that ensures @@ -53,13 +59,21 @@ namespace components /// instantiation that is guaranteed to outlive /// them. static std::shared_ptr - MakeShared(FrameT _frame, ReferenceT _reference); + MakeShared(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units = ReferenceUnits::RADIANS, + bool _ignoreTimeStep = false); /// \brief Environmental data frame. FrameT frame; /// \brief Spatial reference for data coordinates. ReferenceT reference; + + /// \brief The units to be used (only for spherical coordinates) + ReferenceUnits units; + + /// \brief Use time axis or not. + bool staticTime; }; /// \brief A component type that contains a environment data. diff --git a/src/Util.cc b/src/Util.cc index 56f0fca50f..9b786d43df 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,45 @@ 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 + ) +{ + + auto origin = + _ecm.Component(worldEntity(_ecm)); + if (!origin) + { + if (_gridField->reference == math::SphericalCoordinates::SPHERICAL) + { + // If the reference frame is spherical, we must have some world reference + // coordinates. + gzerr << "World has no spherical coordinates," + << " but data was loaded with spherical reference plane" + << std::endl; + return std::nullopt; + } + else + { + // No need to transform + return _worldPosition; + } + } + auto position = origin->Data().PositionTransform( + _worldPosition, math::SphericalCoordinates::LOCAL2, + _gridField->reference); + if (_gridField->reference == math::SphericalCoordinates::SPHERICAL && + _gridField->units == components::EnvironmentalData::ReferenceUnits::DEGREES) + { + position.X(GZ_RTOD(position.X())); + position.Y(GZ_RTOD(position.Y())); + } + return position; +} + ////////////////////////////////////////////////// // Getting the first .sdf file in the path std::string findFuelResourceSdf(const std::string &_path) diff --git a/src/components/Environment.cc b/src/components/Environment.cc index 65232966c4..ccd498688d 100644 --- a/src/components/Environment.cc +++ b/src/components/Environment.cc @@ -23,10 +23,13 @@ using namespace gz::sim::components; std::shared_ptr -EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference) +EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units, bool _ignoreTimeStep) { auto data = std::make_shared(); data->frame = std::move(_frame); data->reference = _reference; + data->units = _units; + data->staticTime = _ignoreTimeStep; return data; } diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 7d092c36dc..956158f39c 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -136,6 +136,7 @@ add_subdirectory(copy_paste) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) add_subdirectory(environment_loader) +add_subdirectory(environment_visualization) add_subdirectory(joint_position_controller) add_subdirectory(lights) add_subdirectory(playback_scrubber) diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index c7ec575e0f..e88d76ea1c 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -64,6 +64,9 @@ class EnvironmentLoaderPrivate /// \brief Index of data dimension to be used as z coordinate. public: int zIndex{-1}; + /// \brief Index of data dimension to be used as units. + public: QString unit; + public: using ReferenceT = math::SphericalCoordinates::CoordinateType; /// \brief Map of supported spatial references. @@ -72,6 +75,15 @@ class EnvironmentLoaderPrivate {QString("spherical"), math::SphericalCoordinates::SPHERICAL}, {QString("ecef"), math::SphericalCoordinates::ECEF}}; + /// \brief Map of supported spatial units. + public: const QMap + unitMap{ + {QString("degree"), + components::EnvironmentalData::ReferenceUnits::DEGREES}, + {QString("radians"), + components::EnvironmentalData::ReferenceUnits::RADIANS} + }; + /// \brief Spatial reference. public: QString reference; @@ -131,7 +143,8 @@ void EnvironmentLoader::Update(const UpdateInfo &, static_cast(this->dataPtr->xIndex), static_cast(this->dataPtr->yIndex), static_cast(this->dataPtr->zIndex)}), - this->dataPtr->referenceMap[this->dataPtr->reference]); + this->dataPtr->referenceMap[this->dataPtr->reference], + this->dataPtr->unitMap[this->dataPtr->unit]); using ComponentT = components::Environment; _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); @@ -210,6 +223,29 @@ QStringList EnvironmentLoader::DimensionList() const return this->dataPtr->dimensionList; } +///////////////////////////////////////////////// +QStringList EnvironmentLoader::UnitList() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->unitMap.keys(); +} + +///////////////////////////////////////////////// +QString EnvironmentLoader::Unit() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->unit; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetUnit(QString _unit) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->unit = _unit; + } + this->IsConfiguredChanged(); +} ///////////////////////////////////////////////// int EnvironmentLoader::TimeIndex() const { diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.hh b/src/gui/plugins/environment_loader/EnvironmentLoader.hh index 08ab2b773c..6a77d1e00c 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.hh +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.hh @@ -55,6 +55,20 @@ inline namespace GZ_SIM_VERSION_NAMESPACE NOTIFY DimensionListChanged ) + /// \brief Unit list + Q_PROPERTY( + QStringList unitList + READ UnitList + ) + + /// \brief Unit list + Q_PROPERTY( + QString unit + READ Unit + WRITE SetUnit + NOTIFY UnitChanged + ) + /// \brief Time index Q_PROPERTY( int timeIndex @@ -136,6 +150,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Get dimensions available in the data file public: Q_INVOKABLE QStringList DimensionList() const; + /// \brief Get available units + public: Q_INVOKABLE QStringList UnitList() const; + /// \brief Notify that the list of dimensions has changed signals: void DimensionListChanged(); @@ -187,6 +204,15 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Notify the spatial reference has changed signals: void ReferenceChanged() const; + /// \brief Get index of the unit in the list + public: Q_INVOKABLE QString Unit() const; + + /// \brief Set index of the unit in the list + public: Q_INVOKABLE void SetUnit(QString _unit); + + /// \brief Notify the unit index has changed + signals: void UnitChanged() const; + /// \brief Get configuration status public: Q_INVOKABLE bool IsConfigured() const; diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.qml b/src/gui/plugins/environment_loader/EnvironmentLoader.qml index cd8b1001f6..f56ec83db0 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.qml +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.qml @@ -26,7 +26,7 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 400 + Layout.minimumHeight: 500 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -203,9 +203,34 @@ GridLayout { ToolTip.text: qsTr("Reference for spatial dimensions") } + Label { + Layout.row: 6 + Layout.columnSpan: 1 + horizontalAlignment: Text.AlignRight + id: unitText + color: "dimgrey" + text: qsTr("Units") + } + + ComboBox { + Layout.row: 6 + Layout.column: 2 + Layout.columnSpan: 6 + id: unitsConversion + Layout.fillWidth: true + enabled: referenceCombo.currentIndex == 2 + model: EnvironmentLoader.unitList + onCurrentTextChanged: { + EnvironmentLoader.unit = currentText + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Units") + } + + Button { text: qsTr("Load") - Layout.row: 6 + Layout.row: 7 Layout.columnSpan: 8 Layout.fillWidth: true enabled: EnvironmentLoader.configured diff --git a/src/gui/plugins/environment_visualization/CMakeLists.txt b/src/gui/plugins/environment_visualization/CMakeLists.txt new file mode 100644 index 0000000000..893365700d --- /dev/null +++ b/src/gui/plugins/environment_visualization/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(EnvironmentVisualization + SOURCES EnvironmentVisualization.cc + QT_HEADERS EnvironmentVisualization.hh + PRIVATE_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-common${GZ_COMMON_VER}::io + gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} +) diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc new file mode 100644 index 0000000000..fbfe9bfed0 --- /dev/null +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -0,0 +1,313 @@ +/* + * 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 "EnvironmentVisualization.hh" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +using namespace gz; +using namespace sim; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +/// \brief Private data class for EnvironmentVisualization +class EnvironmentVisualizationPrivate +{ + 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) + { + 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; + } + } + + ///////////////////////////////////////////////// + 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() + { + 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 xSamples, double ySamples, double 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 = + ceil(xSamples) * ceil(ySamples) * ceil(zSamples); + std::size_t dataSize{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 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(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 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("")); + } + } + + 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; +}; +} +} +} + +///////////////////////////////////////////////// +EnvironmentVisualization::EnvironmentVisualization() + : GuiSystem(), dataPtr(new EnvironmentVisualizationPrivate) +{ + gui::App()->Engine()->rootContext()->setContextProperty( + "EnvironmentVisualization", this); +} + +///////////////////////////////////////////////// +EnvironmentVisualization::~EnvironmentVisualization() +{ +} + +///////////////////////////////////////////////// +void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Environment Visualization Resolution"; + + gui::App()->findChild()->installEventFilter(this); +} + +///////////////////////////////////////////////// +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->xSamples, this->ySamples, this->zSamples + ); +} + +///////////////////////////////////////////////// +void EnvironmentVisualization::ResamplePointcloud() +{ + 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 new file mode 100644 index 0000000000..bf02beb00c --- /dev/null +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -0,0 +1,80 @@ +/* + * 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_GUI_ENVIRONMENTVISUALIZATION_HH_ +#define GZ_SIM_GUI_ENVIRONMENTVISUALIZATION_HH_ + +#include + +#include "gz/sim/gui/GuiSystem.hh" +#include "gz/gui/qt.h" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + class EnvironmentVisualizationPrivate; + + /// \class EnvironmentVisualization EnvironmentVisualization.hh + /// gz/sim/systems/EnvironmentVisualization.hh + /// \brief A GUI plugin for a user to load an Environment + /// component into the ECM on a live simulation. + class EnvironmentVisualization : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Samples along x axis + Q_PROPERTY(qreal xSamples MEMBER xSamples) + + /// \brief Samples along y axis + Q_PROPERTY(qreal ySamples MEMBER ySamples) + + /// \brief Samples along z axis + Q_PROPERTY(qreal zSamples MEMBER zSamples) + + /// \brief Constructor + public: EnvironmentVisualization(); + + /// \brief Destructor + public: ~EnvironmentVisualization() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &_ecm) override; + + public: Q_INVOKABLE void ResamplePointcloud(); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + + public: qreal xSamples{10}; + + public: qreal ySamples{10}; + + public: qreal zSamples{10}; + }; +} +} +} +#endif diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml new file mode 100644 index 0000000000..2fbdae8030 --- /dev/null +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qml @@ -0,0 +1,111 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 400 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + Label { + Layout.row: 0 + Layout.columnSpan: 8 + horizontalAlignment: Text.AlignCenter + id: instructionLabel + color: "dimgrey" + text: qsTr("For the actual pointcloud please open the point_cloud panel") + } + + Label { + Layout.row: 1 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: dimensionLabelX + color: "dimgrey" + text: qsTr("X Samples") + } + + Slider { + Layout.row: 1 + Layout.column: 2 + Layout.columnSpan: 6 + id: stepSliderX + from: 5 + value: 10 + to: 50 + onMoved: function() { + EnvironmentVisualization.xSamples = value; + EnvironmentVisualization.ResamplePointcloud(); + } + } + + Label { + Layout.row: 2 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: dimensionLabelY + color: "dimgrey" + text: qsTr("Y Samples") + } + + Slider { + Layout.row: 2 + Layout.column: 2 + Layout.columnSpan: 6 + id: stepSliderY + from: 5 + value: 10 + to: 50 + onMoved: function() { + EnvironmentVisualization.ySamples = value; + EnvironmentVisualization.ResamplePointcloud(); + } + } + + Label { + Layout.row: 3 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: dimensionLabelZ + color: "dimgrey" + text: qsTr("Z Samples") + } + + Slider { + Layout.row: 3 + Layout.column: 2 + Layout.columnSpan: 6 + id: stepSliderZ + from: 5 + value: 10 + to: 50 + onMoved: function() { + EnvironmentVisualization.zSamples = value; + EnvironmentVisualization.ResamplePointcloud(); + } + } +} diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.qrc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qrc new file mode 100644 index 0000000000..db66d95327 --- /dev/null +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.qrc @@ -0,0 +1,5 @@ + + + EnvironmentVisualization.qml + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 352a00c1fa..84ad0018ae 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -115,6 +115,7 @@ if (NOT WIN32) add_subdirectory(elevator) endif() add_subdirectory(environment_preload) +add_subdirectory(environmental_sensor_system) add_subdirectory(follow_actor) add_subdirectory(force_torque) add_subdirectory(hydrodynamics) diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 92926d0746..fe5b57f940 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -93,14 +93,20 @@ void EnvironmentPreload::PreUpdate( return; } + components::EnvironmentalData::ReferenceUnits unit{ + components::EnvironmentalData::ReferenceUnits::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"); if (elem) { + if (elem->HasElement("ignore_time")) + { + ignoreTime = elem->Get("ignore_time"); + } if (elem->HasElement("time")) { timeColumnName = elem->Get("time"); @@ -119,6 +125,18 @@ void EnvironmentPreload::PreUpdate( else if (referenceName == "spherical") { spatialReference = math::SphericalCoordinates::SPHERICAL; + if (elem->HasAttribute("units")) + { + std::string unitName = elem->Get("units"); + if (unitName == "degrees") + { + unit = components::EnvironmentalData::ReferenceUnits::DEGREES; + } + else if (unitName != "radians") + { + ignerr << "Unrecognized unit " << unitName << "\n"; + } + } } else if (referenceName == "ecef") { @@ -144,13 +162,14 @@ void EnvironmentPreload::PreUpdate( try { + gzmsg << "Loading Environment Data\n"; using ComponentDataT = components::EnvironmentalData; auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), timeColumnName, spatialColumnNames), - spatialReference); + spatialReference, unit, ignoreTime); using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; diff --git a/src/systems/environmental_sensor_system/CMakeLists.txt b/src/systems/environmental_sensor_system/CMakeLists.txt new file mode 100644 index 0000000000..89f30b71dd --- /dev/null +++ b/src/systems/environmental_sensor_system/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(environmental-sensor + SOURCES + EnvironmentalSensorSystem.cc + PUBLIC_LINK_LIBS + gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-math${GZ_MATH_VER}::eigen3 +) diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc new file mode 100644 index 0000000000..b1694e97a4 --- /dev/null +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc @@ -0,0 +1,359 @@ +/* + * 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 "EnvironmentalSensorSystem.hh" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +using namespace gz; +using namespace gz::sim; + +/// Sensor prefix to be used. All envionment_sensors are to be prefixed by +/// "environment_sensor/" in their gz:type field. +constexpr char SENSOR_TYPE_PREFIX[] = "environmental_sensor/"; + +//////////////////////////////////////////////////////////////// +/// \brief Envirtonment Sensor used for looking up environment values in our +/// CSV file. +class EnvironmentalSensor : public gz::sensors::Sensor +{ + /// \brief Node for communication + private: gz::transport::Node node; + + /// \brief Publishes sensor data + private: gz::transport::Node::Publisher pub; + + //////////////////////////////////////////////////////////////// + /// Documentation inherited + public: virtual bool Load(const sdf::Sensor &_sdf) override + { + auto type = gz::sensors::customType(_sdf); + if (!common::StartsWith(type, SENSOR_TYPE_PREFIX)) + { + gzerr << "Trying to load [" << SENSOR_TYPE_PREFIX + << "] sensor, but got type [" + << type << "] instead." << std::endl; + return false; + } + + // Load common sensor params + gz::sensors::Sensor::Load(_sdf); + + this->pub = this->node.Advertise(this->Topic()); + + // If "environment_variable" is defined then remap + // sensor from existing data. + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("environment_variable")) + { + this->fieldName = + _sdf.Element()->Get("environment_variable"); + } + else + { + this->fieldName = type.substr(strlen(SENSOR_TYPE_PREFIX)); + } + + // Allow setting custom frame_ids + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("frame_id")) + { + this->frameId = _sdf.Element()->Get("frame_id"); + } + + gzdbg << "Loaded environmental sensor for " << this->fieldName + << " publishing on " << this->Topic() << std::endl; + + return true; + } + + //////////////////////////////////////////////////////////////// + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return True if the update was successful + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override + { + if (!this->ready) return false; + + if (!this->session.has_value()) return false; + + // Step time if its not static + if (!this->gridField->staticTime) + this->session = this->gridField->frame[this->fieldName].StepTo( + this->session.value(), std::chrono::duration(_now).count()); + + if (!this->session.has_value()) return false; + + gz::msgs::Double msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value((this->frameId == "") ? this->Name() : this->frameId); + auto data = this->gridField->frame[this->fieldName].LookUp( + this->session.value(), this->position); + if (!data.has_value()) + { + gzwarn << "Failed to acquire value perhaps out of field?\n"; + return false; + } + msg.set_data(data.value()); + // TODO(anyone) Add sensor noise. + this->pub.Publish(msg); + return true; + } + + //////////////////////////////////////////////////////////////// + /// \brief Attempts to set a data table. + /// \param[in] _data - The data table + /// \param[in] _curr_time - The current time. + public: void SetDataTable( + const components::Environment* _data, + const std::chrono::steady_clock::duration &_curr_time) + { + gzdbg << "Setting new data table\n"; + auto data = _data->Data(); + if(!data->frame.Has(this->fieldName)) + { + gzwarn << "Environmental sensor could not find field " + << this->fieldName << "\n"; + this->ready = false; + return; + } + + this->gridField = data; + this->session = this->gridField->frame[this->fieldName].CreateSession(); + if (!this->gridField->staticTime) + { + this->session = this->gridField->frame[this->fieldName].StepTo( + *this->session, + std::chrono::duration(_curr_time).count()); + } + this->ready = true; + + if(!this->session.has_value()) + { + gzerr << "Exceeded time stamp." << std::endl; + } + } + + //////////////////////////////////////////////////////////////// + /// \brief Updates the position of the sensor at each time step + /// to track a given entity, + /// \param[in] - The sensor entity. + /// \param[in] - The ECM to use for tracking. + /// \returns True if successful, false if not. + public: bool UpdatePosition( + const Entity _entity, + const EntityComponentManager& _ecm) + { + if (!this->ready) return false; + + const auto position = worldPose(_entity, _ecm).Pos(); + auto lookupCoords = + getGridFieldCoordinates(_ecm, position, this->gridField); + + if (!lookupCoords.has_value()) + { + return false; + } + + this->position = lookupCoords.value(); + return true; + } + + //////////////////////////////////////////////////////////////// + public: std::string Field() const + { + return fieldName; + } + + private: bool ready {false}; + private: math::Vector3d position; + private: std::string fieldName; + private: std::string frameId; + private: std::optional> session; + private: std::shared_ptr + gridField; +}; + +class gz::sim::EnvironmentalSensorSystemPrivate { + public: std::unordered_map> + entitySensorMap; + + public: std::unordered_set fields; + + public: void RemoveSensorEntities( + const gz::sim::EntityComponentManager &_ecm) + { + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::CustomSensor *)->bool + { + if (this->entitySensorMap.erase(_entity) == 0) + { + gzerr << "Internal error, missing environment sensor for entity [" + << _entity << "]" << std::endl; + } + return true; + }); + } + + public: Entity worldEntity; + + public: bool useSphericalCoords{false}; +}; + + +//////////////////////////////////////////////////////////////// +EnvironmentalSensorSystem::EnvironmentalSensorSystem () : + dataPtr(std::make_unique()) +{ + +} + +//////////////////////////////////////////////////////////////// +void EnvironmentalSensorSystem::Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &/*_ecm*/, + gz::sim::EventManager &/*_eventMgr*/) +{ + dataPtr->worldEntity = _entity; +} + +//////////////////////////////////////////////////////////////// +void EnvironmentalSensorSystem::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const Entity &_entity, + const components::CustomSensor *_custom, + const components::ParentEntity *_parent)->bool + { + // Get sensor's scoped name without the world + auto sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + auto type = gz::sensors::customType(data); + if (!common::StartsWith(type, SENSOR_TYPE_PREFIX)) + { + return true; + } + + // Default to scoped name as topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/" + type; + data.SetTopic(topic); + } + + gz::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + + // Set sensor parent + auto parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // Set topic on Gazebo + _ecm.CreateComponent(_entity, + components::SensorTopic(sensor->Topic())); + + // Get current EnvironmentalData component + auto environData = + _ecm.Component( + worldEntity(_ecm)); + + if (environData != nullptr) + { + sensor->SetDataTable(environData, _info.simTime); + } + else + { + gzerr << "No sensor data loaded\n"; + } + + // Keep track of this sensor + this->dataPtr->entitySensorMap.insert(std::make_pair(_entity, + std::move(sensor))); + return true; + }); +} + +//////////////////////////////////////////////////////////////// +void EnvironmentalSensorSystem::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachNew([&](const Entity &/*_entity*/, + const components::Environment *_environmental_data)->bool + { + for (auto &[entity, sensor] : this->dataPtr->entitySensorMap) + { + sensor->SetDataTable(_environmental_data, _info.simTime); + } + return true; + }); + // Only update and publish if not paused. + if (!_info.paused) + { + + for (auto &[entity, sensor] : this->dataPtr->entitySensorMap) + { + sensor->UpdatePosition(entity, _ecm); + sensor->Update(_info.simTime); + } + } + + this->dataPtr->RemoveSensorEntities(_ecm); +} + +GZ_ADD_PLUGIN( + EnvironmentalSensorSystem, System, + EnvironmentalSensorSystem::ISystemConfigure, + EnvironmentalSensorSystem::ISystemPreUpdate, + EnvironmentalSensorSystem::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS( + EnvironmentalSensorSystem, + "gz::sim::systems::EnvironmentalSystem") diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh new file mode 100644 index 0000000000..ecb7ad9845 --- /dev/null +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh @@ -0,0 +1,68 @@ +/* + * 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_ENVIRONMENTAL_SYSTEM_HH_ +#define GZ_ENVIRONMENTAL_SYSTEM_HH_ + +#include +#include +#include + +#include + +namespace gz +{ +namespace sim +{ +class EnvironmentalSensorSystemPrivate; +/// \brief Sensor for reading environmental data loaded from outside world. +/// To use, add this system to the world file, then instantiate sensors of +/// custom type with gz:type="environmental_sensor/{field_name}", where +/// field_name refers to the type of data you would like to output. +/// Alternatively, if you would like to specify a custom field name you may do +/// so using the tag. +class EnvironmentalSensorSystem: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ + public: EnvironmentalSensorSystem(); + /// Documentation inherited + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/) override; + + // Documentation inherited. + // During PreUpdate, check for new sensors that were inserted + // into simulation and create more components as needed. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + // Documentation inherited. + // During PostUpdate, update the known sensors and publish their data. + // Also remove sensors that have been deleted. + public: void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + + private: std::unique_ptr dataPtr; +}; +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 1ee62ed9b7..0e15ef4f04 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -20,6 +20,7 @@ set(tests entity_erase.cc entity_system.cc environment_preload_system.cc + environmental_sensor_system.cc events.cc examples_build.cc follow_actor_system.cc diff --git a/test/integration/environment_preload_system.cc b/test/integration/environment_preload_system.cc index bc4ee515d2..84e5f0178c 100644 --- a/test/integration/environment_preload_system.cc +++ b/test/integration/environment_preload_system.cc @@ -18,12 +18,12 @@ #include #include +#include #include "gz/sim/components/Environment.hh" #include "gz/sim/Server.hh" #include "gz/sim/TestFixture.hh" - -#include +#include "gz/sim/Util.hh" #include "test_config.hh" #include "../helpers/EnvTestFixture.hh" @@ -73,12 +73,69 @@ TEST_F(EnvironmentPreloadTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) { const math::Vector3d position{36.80079505, -121.789472517, 0.8}; auto humidity = - humidityData.LookUp(humiditySession.value(), position); + humidityData.LookUp(humiditySession.value(), + position); EXPECT_NEAR(89.5, humidity.value_or(0.), 1e-6); dataLoaded = true; } EXPECT_EQ(data->reference, math::SphericalCoordinates::SPHERICAL); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server + server.RunOnce(); + + EXPECT_TRUE(dataLoaded); +} + +///////////////////////////////////////////////// +TEST_F(EnvironmentPreloadTest, CorrectSphericalTransform) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "test", + "worlds", "environmental_data.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + bool dataLoaded{false}; + + // Create a system that looks for environmental data components + test::Relay testSystem; + testSystem.OnPostUpdate( + [&](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](const gz::sim::Entity &, + const components::Environment *_component) -> bool + { + auto data = _component->Data(); + EXPECT_TRUE(data->frame.Has("humidity")); + const auto &humidityData = data->frame["humidity"]; + auto humiditySession = humidityData.StepTo( + humidityData.CreateSession(), 1658923062.5); + EXPECT_TRUE(humiditySession.has_value()); + if (humiditySession.has_value()) + { + const math::Vector3d position{0, 0, 0.8}; + auto transformedCoordinates = + getGridFieldCoordinates(_ecm, position, data); + auto humidity = + humidityData.LookUp(humiditySession.value(), + transformedCoordinates.value()); + EXPECT_NEAR(89.5, humidity.value_or(0.), 1e-6); + dataLoaded = true; + } + EXPECT_EQ(data->reference, math::SphericalCoordinates::SPHERICAL); return true; }); }); diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc new file mode 100644 index 0000000000..73660c9d28 --- /dev/null +++ b/test/integration/environmental_sensor_system.cc @@ -0,0 +1,83 @@ +/* + * 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 + +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/TestFixture.hh" + +#include + +#include "gz/transport/Node.hh" +#include "test_config.hh" +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test EnvironmentPreload system +class EnvironmentSensorTest : public InternalFixture<::testing::Test> +{ + ///////////////////////////////////////////////// + public: EnvironmentSensorTest() + { + node.Subscribe("/world/environmental_sensor_test/" + "model/model_with_sensor/link/link/sensor/custom_sensor/" + "environmental_sensor/humidity", + &EnvironmentSensorTest::OnReceiveMsg, + this); + } + + ///////////////////////////////////////////////// + public: void OnReceiveMsg(const gz::msgs::Double& msg) + { + // Data set is such that sensor value == time for the first second. + double nsec = msg.header().stamp().nsec(); + double sec = msg.header().stamp().sec(); + auto time = nsec * 1e-9 + sec; + EXPECT_NEAR(time, msg.data(), 1e-9); + this->receivedData = true; + } + + ///////////////////////////////////////////////// + public: std::atomic receivedData{false}; + + ///////////////////////////////////////////////// + public: transport::Node node; + +}; + +///////////////////////////////////////////////// +TEST_F(EnvironmentSensorTest, CanPreload) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "test", + "worlds", "environmental_sensor.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + // Run server + server.Run(true, 1000, false); + EXPECT_TRUE(this->receivedData.load()); +} diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index cddef0af3b..dbc095a2c7 100644 --- a/test/worlds/CMakeLists.txt +++ b/test/worlds/CMakeLists.txt @@ -3,3 +3,5 @@ configure_file (mesh.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/mesh.sdf) configure_file (thermal.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/thermal.sdf) configure_file (heightmap.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/heightmap.sdf) configure_file (environmental_data.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_data.sdf) + +configure_file (environmental_sensor.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_sensor.sdf) diff --git a/test/worlds/environmental_data.sdf.in b/test/worlds/environmental_data.sdf.in index ac3ec2a73d..47cc7e2cb2 100644 --- a/test/worlds/environmental_data.sdf.in +++ b/test/worlds/environmental_data.sdf.in @@ -28,13 +28,24 @@ -0.5 0.1 -0.9 + + + EARTH_WGS84 + ENU + 36.80079505 + -121.789472517 + 0 + 0 + + + @CMAKE_SOURCE_DIR@/test/worlds/environmental_data.csv - + latitude longitude altitude diff --git a/test/worlds/environmental_sensor.csv b/test/worlds/environmental_sensor.csv new file mode 100644 index 0000000000..3ec2c0a571 --- /dev/null +++ b/test/worlds/environmental_sensor.csv @@ -0,0 +1,17 @@ +timestamp,humidity,x,y,z +0,0,-1,-1,-1 +0,0,-1,-1, 1 +0,0,-1, 1,-1 +0,0,-1, 1, 1 +0,0, 1,-1,-1 +0,0, 1,-1, 1 +0,0, 1, 1,-1 +0,0, 1, 1, 1 +1,1,-1,-1,-1 +1,1,-1,-1, 1 +1,1,-1, 1,-1 +1,1,-1, 1, 1 +1,1, 1,-1,-1 +1,1, 1,-1, 1 +1,1, 1, 1,-1 +1,1, 1, 1, 1 diff --git a/test/worlds/environmental_sensor.sdf.in b/test/worlds/environmental_sensor.sdf.in new file mode 100644 index 0000000000..46b3673ea7 --- /dev/null +++ b/test/worlds/environmental_sensor.sdf.in @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + @CMAKE_SOURCE_DIR@/test/worlds/environmental_sensor.csv + + + + 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 + true + + + + + + From 5ce1d4980f1e7b570d6b2f348029582ea0d33b19 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 17 Nov 2022 08:19:43 +0800 Subject: [PATCH 02/11] Address compiler warnings. Signed-off-by: Arjo Chakravarty --- .../environment_visualization/EnvironmentVisualization.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index fbfe9bfed0..2706e82e6c 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -183,7 +183,8 @@ class EnvironmentVisualizationPrivate {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); auto numberOfPoints = ceil(xSamples) * ceil(ySamples) * ceil(zSamples); - std::size_t dataSize{numberOfPoints * pcMsg.point_step()}; + std::size_t dataSize{ + static_cast(numberOfPoints * pcMsg.point_step())}; pcMsg.mutable_data()->resize(dataSize); pcMsg.set_height(1); pcMsg.set_width(numberOfPoints); @@ -279,8 +280,8 @@ void EnvironmentVisualization::Update(const UpdateInfo &_info, { _ecm.EachNew( [this]( - const Entity &_entity, - const components::Environment* environment + const Entity &/*_entity*/, + const components::Environment* /*environment*/ ) { this->dataPtr->resample = true; return true; From 0042fec6a17623838f861f6b00200b7d883d5a03 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 17 Nov 2022 08:25:17 +0800 Subject: [PATCH 03/11] More compiler warnings fixed Signed-off-by: Arjo Chakravarty --- .../EnvironmentalSensorSystem.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc index b1694e97a4..85268b7fce 100644 --- a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc @@ -186,9 +186,9 @@ class EnvironmentalSensor : public gz::sensors::Sensor { if (!this->ready) return false; - const auto position = worldPose(_entity, _ecm).Pos(); + const auto worldPosition = worldPose(_entity, _ecm).Pos(); auto lookupCoords = - getGridFieldCoordinates(_ecm, position, this->gridField); + getGridFieldCoordinates(_ecm, worldPosition, this->gridField); if (!lookupCoords.has_value()) { @@ -252,7 +252,7 @@ EnvironmentalSensorSystem::EnvironmentalSensorSystem () : //////////////////////////////////////////////////////////////// void EnvironmentalSensorSystem::Configure( const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, + const std::shared_ptr &/*_sdf*/, gz::sim::EntityComponentManager &/*_ecm*/, gz::sim::EventManager &/*_eventMgr*/) { From 38a887aa40617ab005ec266f10bb33928eb14bba Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 17 Nov 2022 20:26:05 +0800 Subject: [PATCH 04/11] Fix link error on :window: s Signed-off-by: Arjo Chakravarty --- include/gz/sim/Util.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 11e9da8db3..631455bb83 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -297,7 +297,7 @@ namespace gz /// \param[in] _ecm Entity Component Manager /// \param[in] _worldPosition world position /// \param[in] _gridField Gridfield you are interested in. - std::optional getGridFieldCoordinates( + std::optional GZ_SIM_VISIBLE getGridFieldCoordinates( const EntityComponentManager &_ecm, const math::Vector3d& _worldPosition, const std::shared_ptr& _gridField); From 61b150eb08b5c916ae373a7c2ead4c40099db652 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 17 Nov 2022 23:35:11 +0800 Subject: [PATCH 05/11] Try to fix the ABI breakage Signed-off-by: Arjo Chakravarty --- include/gz/sim/components/Environment.hh | 14 +++++++++++--- src/components/Environment.cc | 11 +++++++++-- .../environment_loader/EnvironmentLoader.cc | 5 +++-- .../environment_preload/EnvironmentPreload.cc | 2 +- test/worlds/environmental_data.sdf.in | 2 -- 5 files changed, 24 insertions(+), 10 deletions(-) diff --git a/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh index 57c84a4cf0..305138ab59 100644 --- a/include/gz/sim/components/Environment.hh +++ b/include/gz/sim/components/Environment.hh @@ -59,9 +59,17 @@ namespace components /// instantiation that is guaranteed to outlive /// them. static std::shared_ptr - MakeShared(FrameT _frame, ReferenceT _reference, - ReferenceUnits _units = ReferenceUnits::RADIANS, - bool _ignoreTimeStep = false); + MakeShared(FrameT _frame, ReferenceT _reference); + + /// \brief Instantiate environmental data. + /// + /// An std::make_shared equivalent that ensures + /// dynamically loaded call sites use a template + /// instantiation that is guaranteed to outlive + /// them. + static std::shared_ptr + MakeSharedWithUnits(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units, bool _ignoreTimeStep); /// \brief Environmental data frame. FrameT frame; diff --git a/src/components/Environment.cc b/src/components/Environment.cc index ccd498688d..3445faa300 100644 --- a/src/components/Environment.cc +++ b/src/components/Environment.cc @@ -23,8 +23,15 @@ using namespace gz::sim::components; std::shared_ptr -EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference, - ReferenceUnits _units, bool _ignoreTimeStep) + EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference) +{ + return EnvironmentalData::MakeSharedWithUnits( + _frame, _reference, ReferenceUnits::RADIANS, false); +} + +std::shared_ptr + EnvironmentalData::MakeSharedWithUnits(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units, bool _ignoreTimeStep) { auto data = std::make_shared(); data->frame = std::move(_frame); diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index e88d76ea1c..ceed7e6984 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -135,7 +135,7 @@ void EnvironmentLoader::Update(const UpdateInfo &, try { using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeShared( + auto data = ComponentDataT::MakeSharedWithUnits( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), @@ -144,7 +144,8 @@ void EnvironmentLoader::Update(const UpdateInfo &, static_cast(this->dataPtr->yIndex), static_cast(this->dataPtr->zIndex)}), this->dataPtr->referenceMap[this->dataPtr->reference], - this->dataPtr->unitMap[this->dataPtr->unit]); + this->dataPtr->unitMap[this->dataPtr->unit], + false); using ComponentT = components::Environment; _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index fe5b57f940..5f3d0b29bb 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -164,7 +164,7 @@ void EnvironmentPreload::PreUpdate( { gzmsg << "Loading Environment Data\n"; using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeShared( + auto data = ComponentDataT::MakeSharedWithUnits( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), diff --git a/test/worlds/environmental_data.sdf.in b/test/worlds/environmental_data.sdf.in index 47cc7e2cb2..6186515ef5 100644 --- a/test/worlds/environmental_data.sdf.in +++ b/test/worlds/environmental_data.sdf.in @@ -28,7 +28,6 @@ -0.5 0.1 -0.9 - EARTH_WGS84 ENU @@ -38,7 +37,6 @@ 0 - From 610d5d6beca2848aebd4f92c3ca341fcfceb8d8d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 07:46:50 +0800 Subject: [PATCH 06/11] Revert "Try to fix the ABI breakage" This reverts commit 61b150eb08b5c916ae373a7c2ead4c40099db652. --- include/gz/sim/components/Environment.hh | 14 +++----------- src/components/Environment.cc | 11 ++--------- .../environment_loader/EnvironmentLoader.cc | 5 ++--- .../environment_preload/EnvironmentPreload.cc | 2 +- test/worlds/environmental_data.sdf.in | 2 ++ 5 files changed, 10 insertions(+), 24 deletions(-) diff --git a/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh index 305138ab59..57c84a4cf0 100644 --- a/include/gz/sim/components/Environment.hh +++ b/include/gz/sim/components/Environment.hh @@ -59,17 +59,9 @@ namespace components /// instantiation that is guaranteed to outlive /// them. static std::shared_ptr - MakeShared(FrameT _frame, ReferenceT _reference); - - /// \brief Instantiate environmental data. - /// - /// An std::make_shared equivalent that ensures - /// dynamically loaded call sites use a template - /// instantiation that is guaranteed to outlive - /// them. - static std::shared_ptr - MakeSharedWithUnits(FrameT _frame, ReferenceT _reference, - ReferenceUnits _units, bool _ignoreTimeStep); + MakeShared(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units = ReferenceUnits::RADIANS, + bool _ignoreTimeStep = false); /// \brief Environmental data frame. FrameT frame; diff --git a/src/components/Environment.cc b/src/components/Environment.cc index 3445faa300..ccd498688d 100644 --- a/src/components/Environment.cc +++ b/src/components/Environment.cc @@ -23,15 +23,8 @@ using namespace gz::sim::components; std::shared_ptr - EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference) -{ - return EnvironmentalData::MakeSharedWithUnits( - _frame, _reference, ReferenceUnits::RADIANS, false); -} - -std::shared_ptr - EnvironmentalData::MakeSharedWithUnits(FrameT _frame, ReferenceT _reference, - ReferenceUnits _units, bool _ignoreTimeStep) +EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference, + ReferenceUnits _units, bool _ignoreTimeStep) { auto data = std::make_shared(); data->frame = std::move(_frame); diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index ceed7e6984..e88d76ea1c 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -135,7 +135,7 @@ void EnvironmentLoader::Update(const UpdateInfo &, try { using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeSharedWithUnits( + auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), @@ -144,8 +144,7 @@ void EnvironmentLoader::Update(const UpdateInfo &, static_cast(this->dataPtr->yIndex), static_cast(this->dataPtr->zIndex)}), this->dataPtr->referenceMap[this->dataPtr->reference], - this->dataPtr->unitMap[this->dataPtr->unit], - false); + this->dataPtr->unitMap[this->dataPtr->unit]); using ComponentT = components::Environment; _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 5f3d0b29bb..fe5b57f940 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -164,7 +164,7 @@ void EnvironmentPreload::PreUpdate( { gzmsg << "Loading Environment Data\n"; using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeSharedWithUnits( + auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), diff --git a/test/worlds/environmental_data.sdf.in b/test/worlds/environmental_data.sdf.in index 6186515ef5..47cc7e2cb2 100644 --- a/test/worlds/environmental_data.sdf.in +++ b/test/worlds/environmental_data.sdf.in @@ -28,6 +28,7 @@ -0.5 0.1 -0.9 + EARTH_WGS84 ENU @@ -37,6 +38,7 @@ 0 + From a18d573df3c87d27dad5d2c59f930bb2c0a1668f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 08:18:41 +0800 Subject: [PATCH 07/11] Fix tests Signed-off-by: Arjo Chakravarty --- src/gui/plugins/environment_loader/EnvironmentLoader.cc | 2 ++ test/integration/environment_preload_system.cc | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index e88d76ea1c..6ac8b464cf 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -128,6 +128,8 @@ void EnvironmentLoader::Update(const UpdateInfo &, 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() diff --git a/test/integration/environment_preload_system.cc b/test/integration/environment_preload_system.cc index 84e5f0178c..61a9fbffcd 100644 --- a/test/integration/environment_preload_system.cc +++ b/test/integration/environment_preload_system.cc @@ -93,7 +93,8 @@ TEST_F(EnvironmentPreloadTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) ///////////////////////////////////////////////// -TEST_F(EnvironmentPreloadTest, CorrectSphericalTransform) +TEST_F(EnvironmentPreloadTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(CorrectSphericalTransform)) { // Start server ServerConfig serverConfig; From 2881d616db2da032e4f230648872b2fe9063d823 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 08:39:55 +0800 Subject: [PATCH 08/11] Fix types so that qt warnings go away Signed-off-by: Arjo Chakravarty --- .../EnvironmentVisualization.cc | 11 +++++------ .../EnvironmentVisualization.hh | 12 ++++++------ 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index 2706e82e6c..3ed5f6f9ce 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -174,15 +174,14 @@ class EnvironmentVisualizationPrivate public: void ResizeCloud( std::shared_ptr data, const EntityComponentManager& _ecm, - double xSamples, double ySamples, double zSamples) + 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 = - ceil(xSamples) * ceil(ySamples) * ceil(zSamples); + auto numberOfPoints = xSamples * ySamples * zSamples; std::size_t dataSize{ static_cast(numberOfPoints * pcMsg.point_step())}; pcMsg.mutable_data()->resize(dataSize); @@ -204,13 +203,13 @@ class EnvironmentVisualizationPrivate gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); - for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++) + 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 < ceil(ySamples); y_steps++) + 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 < ceil(zSamples); z_steps++) + for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++) { auto z = lower_bound.Z() + z_steps * dz; auto coords = getGridFieldCoordinates( diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh index bf02beb00c..91cd420eb9 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -41,13 +41,13 @@ inline namespace GZ_SIM_VERSION_NAMESPACE Q_OBJECT /// \brief Samples along x axis - Q_PROPERTY(qreal xSamples MEMBER xSamples) + Q_PROPERTY(unsigned int xSamples MEMBER xSamples) /// \brief Samples along y axis - Q_PROPERTY(qreal ySamples MEMBER ySamples) + Q_PROPERTY(unsigned int ySamples MEMBER ySamples) /// \brief Samples along z axis - Q_PROPERTY(qreal zSamples MEMBER zSamples) + Q_PROPERTY(unsigned int zSamples MEMBER zSamples) /// \brief Constructor public: EnvironmentVisualization(); @@ -68,11 +68,11 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Pointer to private data private: std::unique_ptr dataPtr; - public: qreal xSamples{10}; + public: unsigned int xSamples{10}; - public: qreal ySamples{10}; + public: unsigned int ySamples{10}; - public: qreal zSamples{10}; + public: unsigned int zSamples{10}; }; } } From 92718b07971f51b237dfd40aabc1a2a86c8b65bb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 09:43:49 +0800 Subject: [PATCH 09/11] Disable test on windows. Signed-off-by: Arjo Chakravarty --- test/integration/environmental_sensor_system.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc index 73660c9d28..d8eb0996dd 100644 --- a/test/integration/environmental_sensor_system.cc +++ b/test/integration/environmental_sensor_system.cc @@ -65,7 +65,7 @@ class EnvironmentSensorTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(EnvironmentSensorTest, CanPreload) +TEST_F(EnvironmentSensorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) { // Start server ServerConfig serverConfig; From bbb7193b3ecedb3a1ce92142760799a5669237ef Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 11:48:23 +0800 Subject: [PATCH 10/11] Whoopsie. Fix compile error Signed-off-by: Arjo Chakravarty --- test/integration/environmental_sensor_system.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc index d8eb0996dd..10e393448d 100644 --- a/test/integration/environmental_sensor_system.cc +++ b/test/integration/environmental_sensor_system.cc @@ -17,6 +17,7 @@ #include #include +#include #include "gz/sim/components/Environment.hh" #include "gz/sim/Server.hh" From fae0438d6f5a317698176dafcf659251fc66c429 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Nov 2022 12:40:41 +0800 Subject: [PATCH 11/11] Remove warning on windows Signed-off-by: Arjo Chakravarty --- test/integration/environmental_sensor_system.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc index 10e393448d..71e4fda198 100644 --- a/test/integration/environmental_sensor_system.cc +++ b/test/integration/environmental_sensor_system.cc @@ -51,7 +51,7 @@ class EnvironmentSensorTest : public InternalFixture<::testing::Test> { // Data set is such that sensor value == time for the first second. double nsec = msg.header().stamp().nsec(); - double sec = msg.header().stamp().sec(); + double sec = static_cast(msg.header().stamp().sec()); auto time = nsec * 1e-9 + sec; EXPECT_NEAR(time, msg.data(), 1e-9); this->receivedData = true;