From 2d19f9312c87752f50a6b9792bccc46519ed940d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 14 Nov 2022 20:08:29 -0300 Subject: [PATCH] Add EnvironmentalData component (#1616) * Add EnvironmentalData component * Add EnvironmentalDataPreload plugin * Add EnvironmentalDataLoader GUI plugin * Add spatial reference to EnvironmentData * Resolve paths relative to SDF parent path * Avoid template instantiation in plugins * Make EnvironmentData visible in Windows * Disable EnvironmentPreload test on Windows Signed-off-by: Michel Hidalgo --- CMakeLists.txt | 1 + include/gz/sim/components/Environment.hh | 77 +++++ src/CMakeLists.txt | 5 + src/components/Environment.cc | 32 ++ src/gui/plugins/CMakeLists.txt | 1 + .../plugins/environment_loader/CMakeLists.txt | 8 + .../environment_loader/EnvironmentLoader.cc | 319 ++++++++++++++++++ .../environment_loader/EnvironmentLoader.hh | 206 +++++++++++ .../environment_loader/EnvironmentLoader.qml | 216 ++++++++++++ .../environment_loader/EnvironmentLoader.qrc | 5 + src/systems/CMakeLists.txt | 1 + .../environment_preload/CMakeLists.txt | 8 + .../environment_preload/EnvironmentPreload.cc | 172 ++++++++++ .../environment_preload/EnvironmentPreload.hh | 70 ++++ test/integration/CMakeLists.txt | 1 + .../integration/environment_preload_system.cc | 92 +++++ test/worlds/CMakeLists.txt | 1 + test/worlds/environmental_data.csv | 9 + test/worlds/environmental_data.sdf.in | 45 +++ 19 files changed, 1269 insertions(+) create mode 100644 include/gz/sim/components/Environment.hh create mode 100644 src/components/Environment.cc create mode 100644 src/gui/plugins/environment_loader/CMakeLists.txt create mode 100644 src/gui/plugins/environment_loader/EnvironmentLoader.cc create mode 100644 src/gui/plugins/environment_loader/EnvironmentLoader.hh create mode 100644 src/gui/plugins/environment_loader/EnvironmentLoader.qml create mode 100644 src/gui/plugins/environment_loader/EnvironmentLoader.qrc create mode 100644 src/systems/environment_preload/CMakeLists.txt create mode 100644 src/systems/environment_preload/EnvironmentPreload.cc create mode 100644 src/systems/environment_preload/EnvironmentPreload.hh create mode 100644 test/integration/environment_preload_system.cc create mode 100644 test/worlds/environmental_data.csv create mode 100644 test/worlds/environmental_data.sdf.in diff --git a/CMakeLists.txt b/CMakeLists.txt index cd4d24b1bd..ac5b635f3b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,6 +93,7 @@ gz_find_package(gz-common5 profiler events av + io REQUIRED ) set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) diff --git a/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh new file mode 100644 index 0000000000..a0e721e8cd --- /dev/null +++ b/include/gz/sim/components/Environment.hh @@ -0,0 +1,77 @@ +/* + * 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_ENVIRONMENT_HH_ +#define GZ_SIM_ENVIRONMENT_HH_ + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief Environment data across time and space. This is useful to + /// introduce physical quantities that may be of interest even if not + /// modelled in simulation. + struct GZ_SIM_VISIBLE EnvironmentalData + { + using T = math::InMemoryTimeVaryingVolumetricGrid; + using FrameT = common::DataFrame; + using ReferenceT = math::SphericalCoordinates::CoordinateType; + + /// \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 + MakeShared(FrameT _frame, ReferenceT _reference); + + /// \brief Environmental data frame. + FrameT frame; + + /// \brief Spatial reference for data coordinates. + ReferenceT reference; + }; + + /// \brief A component type that contains a environment data. + /// Ownership is shared to avoid data copies unless necessary. + using Environment = + Component, class EnvironmentalDataTag>; + + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.Environment", Environment) +} +} +} +} + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9ff752e95a..05dc466ccc 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -32,6 +32,10 @@ set(comms_sources comms/MsgManager.cc ) +set(component_sources + components/Environment.cc +) + set(gui_sources ${gui_sources} PARENT_SCOPE @@ -68,6 +72,7 @@ set (sources ${PROTO_PRIVATE_SRC} ${network_sources} ${comms_sources} + ${component_sources} ) set (gtest_sources diff --git a/src/components/Environment.cc b/src/components/Environment.cc new file mode 100644 index 0000000000..65232966c4 --- /dev/null +++ b/src/components/Environment.cc @@ -0,0 +1,32 @@ +/* + * 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 "gz/sim/components/Environment.hh" + +#include +#include + +using namespace gz::sim::components; + +std::shared_ptr +EnvironmentalData::MakeShared(FrameT _frame, ReferenceT _reference) +{ + auto data = std::make_shared(); + data->frame = std::move(_frame); + data->reference = _reference; + return data; +} diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index e79bf59acb..7d092c36dc 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -135,6 +135,7 @@ add_subdirectory(component_inspector_editor) add_subdirectory(copy_paste) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) +add_subdirectory(environment_loader) add_subdirectory(joint_position_controller) add_subdirectory(lights) add_subdirectory(playback_scrubber) diff --git a/src/gui/plugins/environment_loader/CMakeLists.txt b/src/gui/plugins/environment_loader/CMakeLists.txt new file mode 100644 index 0000000000..dbd24887ce --- /dev/null +++ b/src/gui/plugins/environment_loader/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(EnvironmentLoader + SOURCES EnvironmentLoader.cc + QT_HEADERS EnvironmentLoader.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_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc new file mode 100644 index 0000000000..c7ec575e0f --- /dev/null +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -0,0 +1,319 @@ +/* + * 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 "EnvironmentLoader.hh" + +#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 EnvironmentLoader +class EnvironmentLoaderPrivate +{ + /// \brief Path to environmental data file to load. + public: QString dataPath; + + /// \brief List of environmental data dimensions + /// (ie. columns if dealing with CSV data). + public: QStringList dimensionList; + + /// \brief Index of data dimension to be used as time. + public: int timeIndex{-1}; + + /// \brief Index of data dimension to be used as x coordinate. + public: int xIndex{-1}; + + /// \brief Index of data dimension to be used as y coordinate. + public: int yIndex{-1}; + + /// \brief Index of data dimension to be used as z coordinate. + public: int zIndex{-1}; + + public: using ReferenceT = math::SphericalCoordinates::CoordinateType; + + /// \brief Map of supported spatial references. + public: const QMap referenceMap{ + {QString("global"), math::SphericalCoordinates::GLOBAL}, + {QString("spherical"), math::SphericalCoordinates::SPHERICAL}, + {QString("ecef"), math::SphericalCoordinates::ECEF}}; + + /// \brief Spatial reference. + public: QString reference; + + /// \brief To synchronize member access. + public: std::mutex mutex; + + /// \brief Whether to attempt an environmental data load. + public: std::atomic needsLoad{false}; +}; +} +} +} + +///////////////////////////////////////////////// +EnvironmentLoader::EnvironmentLoader() + : GuiSystem(), dataPtr(new EnvironmentLoaderPrivate) +{ + gui::App()->Engine()->rootContext()->setContextProperty( + "EnvironmentLoader", this); +} + +///////////////////////////////////////////////// +EnvironmentLoader::~EnvironmentLoader() +{ +} + +///////////////////////////////////////////////// +void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Environment Loader"; + + gui::App()->findChild()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void EnvironmentLoader::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + if (this->dataPtr->needsLoad) + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->needsLoad = false; + + std::ifstream dataFile(this->dataPtr->dataPath.toStdString()); + gzmsg << "Loading environmental data from " + << this->dataPtr->dataPath.toStdString() + << std::endl; + try + { + using ComponentDataT = components::EnvironmentalData; + auto data = ComponentDataT::MakeShared( + common::IO::ReadFrom( + common::CSVIStreamIterator(dataFile), + common::CSVIStreamIterator(), + this->dataPtr->timeIndex, { + static_cast(this->dataPtr->xIndex), + static_cast(this->dataPtr->yIndex), + static_cast(this->dataPtr->zIndex)}), + this->dataPtr->referenceMap[this->dataPtr->reference]); + + using ComponentT = components::Environment; + _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); + } + catch (const std::invalid_argument &exc) + { + gzerr << "Failed to load environmental data" << std::endl + << exc.what() << std::endl; + } + } +} + +///////////////////////////////////////////////// +void EnvironmentLoader::ScheduleLoad() +{ + this->dataPtr->needsLoad = this->IsConfigured(); +} + +///////////////////////////////////////////////// +QString EnvironmentLoader::DataPath() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->dataPath; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetDataUrl(QUrl _dataUrl) +{ + this->SetDataPath(_dataUrl.path()); +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetDataPath(QString _dataPath) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->dataPath = _dataPath; + + std::ifstream dataFile(_dataPath.toStdString()); + if (!dataFile.is_open()) + { + gzerr << "No environmental data file was found at " + << this->dataPtr->dataPath.toStdString() + << std::endl; + this->dataPtr->dataPath.clear(); + return; + } + const common::CSVIStreamIterator iterator(dataFile); + if (iterator == common::CSVIStreamIterator()) + { + gzerr << "Failed to load environmental data at " + << this->dataPtr->dataPath.toStdString() + << std::endl; + this->dataPtr->dataPath.clear(); + return; + } + const std::vector &header = *iterator; + this->dataPtr->dimensionList.clear(); + this->dataPtr->dimensionList.reserve(header.size()); + for (const std::string &dimension : header) + { + this->dataPtr->dimensionList.push_back( + QString::fromStdString(dimension)); + } + } + + this->DataPathChanged(); + this->DimensionListChanged(); + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +QStringList EnvironmentLoader::DimensionList() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->dimensionList; +} + +///////////////////////////////////////////////// +int EnvironmentLoader::TimeIndex() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->timeIndex; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetTimeIndex(int _timeIndex) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->timeIndex = _timeIndex; + } + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +int EnvironmentLoader::XIndex() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->xIndex; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetXIndex(int _xIndex) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->xIndex = _xIndex; + } + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +int EnvironmentLoader::YIndex() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->yIndex; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetYIndex(int _yIndex) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->yIndex = _yIndex; + } + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +int EnvironmentLoader::ZIndex() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->zIndex; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetZIndex(int _zIndex) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->zIndex = _zIndex; + } + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +QStringList EnvironmentLoader::ReferenceList() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->referenceMap.keys(); +} + +///////////////////////////////////////////////// +QString EnvironmentLoader::Reference() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return this->dataPtr->reference; +} + +///////////////////////////////////////////////// +void EnvironmentLoader::SetReference(QString _reference) +{ + { + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->reference = _reference; + } + this->IsConfiguredChanged(); +} + +///////////////////////////////////////////////// +bool EnvironmentLoader::IsConfigured() const +{ + std::lock_guard lock(this->dataPtr->mutex); + return ( + !this->dataPtr->dataPath.isEmpty() && + this->dataPtr->timeIndex != -1 && + this->dataPtr->xIndex != -1 && + this->dataPtr->yIndex != -1 && + this->dataPtr->zIndex != -1 && + !this->dataPtr->reference.isEmpty()); +} + +// Register this plugin +GZ_ADD_PLUGIN(gz::sim::EnvironmentLoader, gz::gui::Plugin) diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.hh b/src/gui/plugins/environment_loader/EnvironmentLoader.hh new file mode 100644 index 0000000000..08ab2b773c --- /dev/null +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.hh @@ -0,0 +1,206 @@ +/* + * 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_ENVIRONMENTLOADER_HH_ +#define GZ_SIM_GUI_ENVIRONMENTLOADER_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 EnvironmentLoaderPrivate; + + /// \class EnvironmentLoader EnvironmentLoader.hh + /// gz/sim/systems/EnvironmentLoader.hh + /// \brief A GUI plugin for a user to load an Environment + /// component into the ECM on a live simulation. + class EnvironmentLoader : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Data path + Q_PROPERTY( + QString dataPath + READ DataPath + WRITE SetDataPath + NOTIFY DataPathChanged + ) + + /// \brief Dimension list + Q_PROPERTY( + QStringList dimensionList + READ DimensionList + NOTIFY DimensionListChanged + ) + + /// \brief Time index + Q_PROPERTY( + int timeIndex + READ TimeIndex + WRITE SetTimeIndex + NOTIFY TimeIndexChanged + ) + + /// \brief X dimension + Q_PROPERTY( + int xIndex + READ XIndex + WRITE SetXIndex + NOTIFY XIndexChanged + ) + + /// \brief Y dimension + Q_PROPERTY( + int yIndex + READ YIndex + WRITE SetYIndex + NOTIFY YIndexChanged + ) + + /// \brief Z dimension + Q_PROPERTY( + int zIndex + READ ZIndex + WRITE SetZIndex + NOTIFY ZIndexChanged + ) + + /// \brief Spatial reference type list + Q_PROPERTY( + QStringList referenceList + READ ReferenceList + ) + + /// \brief Spatial reference + Q_PROPERTY( + QString reference + READ Reference + WRITE SetReference + NOTIFY ReferenceChanged + ) + + /// \brief Configuration ready + Q_PROPERTY( + bool configured + READ IsConfigured + NOTIFY IsConfiguredChanged + ) + + /// \brief Constructor + public: EnvironmentLoader(); + + /// \brief Destructor + public: ~EnvironmentLoader() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &_ecm) override; + + /// \brief Get path to the data file to be loaded + public: Q_INVOKABLE QString DataPath() const; + + /// \brief Notify that the path to the data file (potentially) changed + signals: void DataPathChanged(); + + /// \brief Set the path to the data file to be loaded + public: Q_INVOKABLE void SetDataPath(QString _dataPath); + + /// \brief Set the URL pointing to the data file to be loaded + public: Q_INVOKABLE void SetDataUrl(QUrl _dataUrl); + + /// \brief Get dimensions available in the data file + public: Q_INVOKABLE QStringList DimensionList() const; + + /// \brief Notify that the list of dimensions has changed + signals: void DimensionListChanged(); + + /// \brief Get index of the time dimension in the list + public: Q_INVOKABLE int TimeIndex() const; + + /// \brief Set index of the time dimension in the list + public: Q_INVOKABLE void SetTimeIndex(int _timeIndex); + + /// \brief Notify the time dimension index has changed + signals: void TimeIndexChanged() const; + + /// \brief Get index of the x dimension in the list + public: Q_INVOKABLE int XIndex() const; + + /// \brief Set index of the x dimension in the list + public: Q_INVOKABLE void SetXIndex(int _xIndex); + + /// \brief Notify the x dimension index has changed + signals: void XIndexChanged() const; + + /// \brief Get index of the y dimension in the list + public: Q_INVOKABLE int YIndex() const; + + /// \brief Notify the y dimension index has changed + signals: void YIndexChanged() const; + + /// \brief Set index of the y dimension in the list + public: Q_INVOKABLE void SetYIndex(int _yIndex); + + /// \brief Get index of the z dimension in the list + public: Q_INVOKABLE int ZIndex() const; + + /// \brief Set index of the z dimension in the list + public: Q_INVOKABLE void SetZIndex(int _zIndex); + + /// \brief Notify the z dimension index has changed + signals: void ZIndexChanged() const; + + /// \brief Get supported spatial references + public: Q_INVOKABLE QStringList ReferenceList() const; + + /// \brief Get spatial reference + public: Q_INVOKABLE QString Reference() const; + + /// \brief Set spatial reference + public: Q_INVOKABLE void SetReference(QString _reference); + + /// \brief Notify the spatial reference has changed + signals: void ReferenceChanged() const; + + /// \brief Get configuration status + public: Q_INVOKABLE bool IsConfigured() const; + + /// \brief Notify configuration status changed + signals: void IsConfiguredChanged(); + + /// \brief Schedule an update + public: Q_INVOKABLE void ScheduleLoad(); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.qml b/src/gui/plugins/environment_loader/EnvironmentLoader.qml new file mode 100644 index 0000000000..cd8b1001f6 --- /dev/null +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.qml @@ -0,0 +1,216 @@ +/* + * 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.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: dataFileText + color: "dimgrey" + text: qsTr("Data file path") + } + + RowLayout { + Layout.column: 2 + Layout.columnSpan: 6 + + TextField { + id: dataFilePathInput + Layout.fillWidth: true + text: EnvironmentLoader.dataPath + placeholderText: qsTr("Path to data file") + onEditingFinished: { + EnvironmentLoader.dataPath = text + } + } + + Button { + id: browseDataFile + Layout.preferredWidth: 20 + display: AbstractButton.IconOnly + text: EnvironmentLoader.dataFileName + onClicked: dataFileDialog.open() + icon.source: "qrc:/Gazebo/images/chevron-right.svg" + ToolTip.visible: hovered + ToolTip.text: qsTr("Browse files...") + } + } + + FileDialog { + Layout.columnSpan: 8 + id: dataFileDialog + title: qsTr("Please choose a data file") + folder: shortcuts.home + visible: false + onAccepted: { + EnvironmentLoader.SetDataUrl(dataFileDialog.fileUrl) + } + onRejected: { + } + Component.onCompleted: visible = false + } + + Label { + Layout.row: 1 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: timeDimensionText + color: "dimgrey" + text: qsTr("Time dimension") + } + + ComboBox { + Layout.row: 1 + Layout.column: 2 + Layout.columnSpan: 6 + id: timeDimensionCombo + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + model: EnvironmentLoader.dimensionList + currentIndex: EnvironmentLoader.timeIndex + onCurrentIndexChanged: { + EnvironmentLoader.timeIndex = currentIndex + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Data field to be used as time dimension") + } + + Label { + Layout.row: 2 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: xDimensionText + color: "dimgrey" + text: qsTr("X dimension") + } + + ComboBox { + Layout.row: 2 + Layout.column: 2 + Layout.columnSpan: 6 + id: xDimensionCombo + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + model: EnvironmentLoader.dimensionList + currentIndex: EnvironmentLoader.xIndex + onCurrentIndexChanged: { + EnvironmentLoader.xIndex = currentIndex + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Data field to be used as x dimension") + } + + Label { + Layout.row: 3 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: yDimensionText + color: "dimgrey" + text: qsTr("Y dimension") + } + + ComboBox { + Layout.row: 3 + Layout.column: 2 + Layout.columnSpan: 6 + id: yDimensionCombo + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + model: EnvironmentLoader.dimensionList + currentIndex: EnvironmentLoader.yIndex + onCurrentIndexChanged: { + EnvironmentLoader.yIndex = currentIndex + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Data field to be used as y dimension") + } + + Label { + Layout.row: 4 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: zDimensionText + color: "dimgrey" + text: qsTr("Z dimension") + } + + ComboBox { + Layout.row: 4 + Layout.column: 2 + Layout.columnSpan: 6 + id: zDimensionCombo + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + model: EnvironmentLoader.dimensionList + currentIndex: EnvironmentLoader.zIndex + onCurrentIndexChanged: { + EnvironmentLoader.zIndex = currentIndex + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Data field to be used as z dimension") + } + + Label { + Layout.row: 5 + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: refText + color: "dimgrey" + text: qsTr("Reference") + } + + ComboBox { + Layout.row: 5 + Layout.column: 2 + Layout.columnSpan: 6 + id: referenceCombo + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + model: EnvironmentLoader.referenceList + onCurrentTextChanged: { + EnvironmentLoader.reference = currentText + } + ToolTip.visible: hovered + ToolTip.text: qsTr("Reference for spatial dimensions") + } + + Button { + text: qsTr("Load") + Layout.row: 6 + Layout.columnSpan: 8 + Layout.fillWidth: true + enabled: EnvironmentLoader.configured + onClicked: function() { + EnvironmentLoader.ScheduleLoad() + } + } +} diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.qrc b/src/gui/plugins/environment_loader/EnvironmentLoader.qrc new file mode 100644 index 0000000000..1f66f199b8 --- /dev/null +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.qrc @@ -0,0 +1,5 @@ + + + EnvironmentLoader.qml + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 92ee225c60..352a00c1fa 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -114,6 +114,7 @@ add_subdirectory(diff_drive) if (NOT WIN32) add_subdirectory(elevator) endif() +add_subdirectory(environment_preload) add_subdirectory(follow_actor) add_subdirectory(force_torque) add_subdirectory(hydrodynamics) diff --git a/src/systems/environment_preload/CMakeLists.txt b/src/systems/environment_preload/CMakeLists.txt new file mode 100644 index 0000000000..178ec5ec32 --- /dev/null +++ b/src/systems/environment_preload/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(environment-preload + SOURCES + EnvironmentPreload.cc + PUBLIC_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/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc new file mode 100644 index 0000000000..92926d0746 --- /dev/null +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -0,0 +1,172 @@ +/* + * 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 +#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; + +////////////////////////////////////////////////// +class gz::sim::systems::EnvironmentPreloadPrivate +{ + public: bool loaded{false}; + + public: std::shared_ptr sdf; +}; + +////////////////////////////////////////////////// +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 &, + gz::sim::EntityComponentManager &_ecm) +{ + if (!std::exchange(this->dataPtr->loaded, true)) + { + if (!this->dataPtr->sdf->HasElement("data")) + { + gzerr << "No environmental data file was specified"; + return; + } + + std::string dataPath = + this->dataPtr->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); + } + + std::ifstream dataFile(dataPath); + if (!dataFile.is_open()) + { + gzerr << "No environmental data file was found at " << dataPath; + return; + } + + std::string timeColumnName{"t"}; + std::array spatialColumnNames{"x", "y", "z"}; + auto spatialReference = math::SphericalCoordinates::GLOBAL; + + sdf::ElementConstPtr elem = + this->dataPtr->sdf->FindElement("dimensions"); + if (elem) + { + 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 = math::SphericalCoordinates::GLOBAL; + } + else if (referenceName == "spherical") + { + spatialReference = math::SphericalCoordinates::SPHERICAL; + } + else if (referenceName == "ecef") + { + spatialReference = math::SphericalCoordinates::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]); + } + } + } + } + + try + { + using ComponentDataT = components::EnvironmentalData; + auto data = ComponentDataT::MakeShared( + common::IO::ReadFrom( + common::CSVIStreamIterator(dataFile), + common::CSVIStreamIterator(), + timeColumnName, spatialColumnNames), + spatialReference); + + using ComponentT = components::Environment; + auto component = ComponentT{std::move(data)}; + _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); + } + catch (const std::invalid_argument &exc) + { + gzerr << "Failed to load environment data" << std::endl + << exc.what() << std::endl; + } + } +} + +// 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/EnvironmentPreload.hh b/src/systems/environment_preload/EnvironmentPreload.hh new file mode 100644 index 0000000000..16a9793cc0 --- /dev/null +++ b/src/systems/environment_preload/EnvironmentPreload.hh @@ -0,0 +1,70 @@ +/* + * 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_HH_ +#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_HH_ + +#include + +#include "gz/sim/config.hh" +#include "gz/sim/System.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + class EnvironmentPreloadPrivate; + + /// \class EnvironmentPreload EnvironmentPreload.hh + /// gz/sim/systems/EnvironmentPreload.hh + /// \brief A plugin to preload an Environment component + /// into the ECM upon simulation start-up. + class EnvironmentPreload : + public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: explicit EnvironmentPreload(); + + /// \brief Destructor + public: ~EnvironmentPreload() override; + + // Documentation inherited + public: void Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + sim::EventManager &_eventMgr) final; + + // Documentation inherited + public: void PreUpdate( + const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f3b611d73f..1ee62ed9b7 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -19,6 +19,7 @@ set(tests each_new_removed.cc entity_erase.cc entity_system.cc + environment_preload_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 new file mode 100644 index 0000000000..bc4ee515d2 --- /dev/null +++ b/test/integration/environment_preload_system.cc @@ -0,0 +1,92 @@ +/* + * 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 "test_config.hh" +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test EnvironmentPreload system +class EnvironmentPreloadTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(EnvironmentPreloadTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) +{ + // 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{36.80079505, -121.789472517, 0.8}; + auto humidity = + 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); +} diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index 70e61f51f3..cddef0af3b 100644 --- a/test/worlds/CMakeLists.txt +++ b/test/worlds/CMakeLists.txt @@ -2,3 +2,4 @@ configure_file (buoyancy.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/buoyancy.sdf) 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) diff --git a/test/worlds/environmental_data.csv b/test/worlds/environmental_data.csv new file mode 100644 index 0000000000..923bd22a53 --- /dev/null +++ b/test/worlds/environmental_data.csv @@ -0,0 +1,9 @@ +timestamp,humidity,latitude,longitude,altitude +1658923062,91,36.80029505,-121.788972517,0.8 +1658923062,88,36.80129505,-121.788972517,0.8 +1658923062,89,36.80029505,-121.789972517,0.8 +1658923062,92,36.80129505,-121.789972517,0.8 +1658923063,90,36.80029505,-121.788972517,0.8 +1658923063,85,36.80129505,-121.788972517,0.8 +1658923063,87,36.80029505,-121.789972517,0.8 +1658923063,94,36.80129505,-121.789972517,0.8 diff --git a/test/worlds/environmental_data.sdf.in b/test/worlds/environmental_data.sdf.in new file mode 100644 index 0000000000..ac3ec2a73d --- /dev/null +++ b/test/worlds/environmental_data.sdf.in @@ -0,0 +1,45 @@ + + + + + 0.001 + 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + @CMAKE_SOURCE_DIR@/test/worlds/environmental_data.csv + + + + latitude + longitude + altitude + + + + +