From 8b18af22a4d389740a575bbc784e50d9f6625f7d Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Tue, 13 Sep 2022 10:30:40 -0700 Subject: [PATCH 1/3] Acoustic comms example world (#1693) * Added acoustic comms example Signed-off-by: Aditya --- .../acoustic_comms_demo/CMakeLists.txt | 14 + .../standalone/acoustic_comms_demo/README.md | 31 ++ .../acoustic_comms_demo.cc | 135 +++++++ examples/worlds/acoustic_comms_demo.sdf | 379 ++++++++++++++++++ 4 files changed, 559 insertions(+) create mode 100644 examples/standalone/acoustic_comms_demo/CMakeLists.txt create mode 100644 examples/standalone/acoustic_comms_demo/README.md create mode 100644 examples/standalone/acoustic_comms_demo/acoustic_comms_demo.cc create mode 100644 examples/worlds/acoustic_comms_demo.sdf diff --git a/examples/standalone/acoustic_comms_demo/CMakeLists.txt b/examples/standalone/acoustic_comms_demo/CMakeLists.txt new file mode 100644 index 0000000000..190b0bdc72 --- /dev/null +++ b/examples/standalone/acoustic_comms_demo/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(gz-sim-acoustic-comms-demo) + +find_package(gz-transport12 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR}) + +find_package(gz-sim7 REQUIRED) +set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) + +add_executable(acoustic_comms_demo acoustic_comms_demo.cc) +target_link_libraries(acoustic_comms_demo + gz-transport${GZ_TRANSPORT_VER}::core + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) diff --git a/examples/standalone/acoustic_comms_demo/README.md b/examples/standalone/acoustic_comms_demo/README.md new file mode 100644 index 0000000000..3400638570 --- /dev/null +++ b/examples/standalone/acoustic_comms_demo/README.md @@ -0,0 +1,31 @@ +# Multi-LRAUV Acoustic comms example + +This example shows the usage of the Acoustic comms plugin on +multiple autonomous underwater vehicles (AUV) with buoyancy, lift drag, and +hydrodynamics plugins. The multiple vehicles are differentiated by namespaces. + +It consists of 3 vehicles, +Triton, Tethys, and Daphne floating side by side. Triton sends +a move command using acoustic comms to the other 2 vehicles, +which start moving on receiving the command. The speed of sound +is purposely slowed down here to show that the middle vehicle (Tethys) +will receive the signal and start moving before Daphne. + +## Build Instructions + +From this directory, run the following to compile: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +From the `build` directory, run Gazebo Sim and the example controller: + + gz sim -r ../../../worlds/acoustic_comms_demo.sdf + ./acoustic_comms_demo + +It can be seen visually that one of the vehicles (Triton) starts moving +immediately, then afer a while Tethys will start moving, and then finally Daphne. diff --git a/examples/standalone/acoustic_comms_demo/acoustic_comms_demo.cc b/examples/standalone/acoustic_comms_demo/acoustic_comms_demo.cc new file mode 100644 index 0000000000..2eeabd3556 --- /dev/null +++ b/examples/standalone/acoustic_comms_demo/acoustic_comms_demo.cc @@ -0,0 +1,135 @@ +/* + * 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. + * + */ + +/* + * Check README for detailed instructions. + * Usage: + * $ gz sim -r worlds/acoustic_comms_demo.sdf + * $ ./acoustic_comms_demo + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace gz; + +int main(int argc, char** argv) +{ + transport::Node node; + + auto propellerTopicTriton = + gz::transport::TopicUtils::AsValidTopic( + "/model/triton/joint/propeller_joint/cmd_pos"); + auto propellerPubTriton = + node.Advertise(propellerTopicTriton); + + auto propellerTopicTethys = + gz::transport::TopicUtils::AsValidTopic( + "/model/tethys/joint/propeller_joint/cmd_pos"); + auto propellerPubTethys = + node.Advertise(propellerTopicTethys); + + auto propellerTopicDaphne = + gz::transport::TopicUtils::AsValidTopic( + "/model/daphne/joint/propeller_joint/cmd_pos"); + auto propellerPubDaphne = + node.Advertise(propellerTopicDaphne); + + double propellerCmdTriton = -20; + double propellerCmdTethys = 0; + double propellerCmdDaphne = 0; + + // Setup publishers and callbacks for comms topics. + auto senderAddressTriton = "1"; + + std::string receiverAddressTethys = "2"; + std::string receiverAddressDaphne = "3"; + + // Set up callbacks + std::atomic tethysMsgReceived = false; + std::atomic daphneMsgReceived = false; + + std::function cbTethys = + [&](const msgs::Dataframe &_msg) + { + tethysMsgReceived = true; + }; + + std::function cbDaphne = + [&](const msgs::Dataframe &_msg) + { + daphneMsgReceived = true; + }; + + node.Subscribe("/" + receiverAddressTethys + "/rx", cbTethys); + node.Subscribe("/" + receiverAddressDaphne + "/rx", cbDaphne); + + // Publisher to send the START msg. + auto pub = node.Advertise( + "/broker/msgs"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Prepare and send the msgs from triton to tethys and daphne + msgs::Dataframe msgTethys; + msgs::Dataframe msgDaphne; + + msgTethys.set_src_address(senderAddressTriton); + msgDaphne.set_src_address(senderAddressTriton); + + msgTethys.set_dst_address(receiverAddressTethys); + msgDaphne.set_dst_address(receiverAddressDaphne); + + msgTethys.set_data("START"); + msgDaphne.set_data("START"); + + while (true) + { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + std::cout << "--------------------------" << std::endl; + if (tethysMsgReceived) + propellerCmdTethys = -20.0; + else + pub.Publish(msgTethys); + + if (daphneMsgReceived) + propellerCmdDaphne = -20.0; + else + pub.Publish(msgDaphne); + + msgs::Double propellerMsg; + propellerMsg.set_data(propellerCmdTriton); + propellerPubTriton.Publish(propellerMsg); + + propellerMsg.set_data(propellerCmdTethys); + propellerPubTethys.Publish(propellerMsg); + + propellerMsg.set_data(propellerCmdDaphne); + propellerPubDaphne.Publish(propellerMsg); + + std::cout << "Commanding thrust: " << std::endl; + std::cout << "triton : " << propellerCmdTriton << " N" << std::endl; + std::cout << "tethys : " << propellerCmdTethys << " N" << std::endl; + std::cout << "daphne : " << propellerCmdDaphne << " N" << std::endl; + } +} diff --git a/examples/worlds/acoustic_comms_demo.sdf b/examples/worlds/acoustic_comms_demo.sdf new file mode 100644 index 0000000000..d5ebe9e0e5 --- /dev/null +++ b/examples/worlds/acoustic_comms_demo.sdf @@ -0,0 +1,379 @@ + + + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + + + + 2500 + 3 + + + + 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 + + + + -5 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + +
2
+ 2/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + 15 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + +
1
+ 1/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + -15 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + +
3
+ 3/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + + From f89705c4ea17cbd3fc1245a962ac45f76b773603 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 28 Sep 2022 14:30:45 +0800 Subject: [PATCH 2/3] add gui Signed-off-by: Arjo Chakravarty --- include/gz/sim/components/Environment.hh | 4 +-- src/components/Environment.cc | 2 +- .../environment_loader/EnvironmentLoader.cc | 36 ++++++++++++++++++- .../environment_loader/EnvironmentLoader.hh | 26 ++++++++++++++ .../environment_loader/EnvironmentLoader.qml | 30 ++++++++++++++-- 5 files changed, 91 insertions(+), 7 deletions(-) diff --git a/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh index 9c766b1130..8267159148 100644 --- a/include/gz/sim/components/Environment.hh +++ b/include/gz/sim/components/Environment.hh @@ -38,7 +38,7 @@ 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. + /// modelled in simulation. struct EnvironmentalData { using T = math::InMemoryTimeVaryingVolumetricGrid; @@ -46,7 +46,7 @@ namespace components using ReferenceT = math::SphericalCoordinates::CoordinateType; /// \brief Reference units - enum ReferenceUnits { + enum class ReferenceUnits { RADIANS = 0, DEGREES }; diff --git a/src/components/Environment.cc b/src/components/Environment.cc index 6daca18177..ccd498688d 100644 --- a/src/components/Environment.cc +++ b/src/components/Environment.cc @@ -23,7 +23,7 @@ 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(); diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index c7ec575e0f..3ba360e967 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,13 @@ 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::DEGREES}, + {QString("radians"), components::EnvironmentalData::RADIANS} + }; + /// \brief Spatial reference. public: QString reference; @@ -131,7 +141,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 +221,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 5e2e7c2fa3..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 @@ -196,7 +196,6 @@ GridLayout { Layout.fillWidth: true enabled: EnvironmentLoader.configured model: EnvironmentLoader.referenceList - currentText: EnvironmentLoader.reference onCurrentTextChanged: { EnvironmentLoader.reference = currentText } @@ -204,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 From ace5012aff6f53a0efd732bd0a5a572179f445ca Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 28 Sep 2022 15:03:44 +0800 Subject: [PATCH 3/3] Address PR feedback, Signed-off-by: Arjo Chakravarty --- include/gz/sim/components/Environment.hh | 3 ++- src/gui/plugins/environment_loader/EnvironmentLoader.cc | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/gz/sim/components/Environment.hh b/include/gz/sim/components/Environment.hh index 8267159148..d814639313 100644 --- a/include/gz/sim/components/Environment.hh +++ b/include/gz/sim/components/Environment.hh @@ -59,7 +59,8 @@ namespace components /// them. static std::shared_ptr MakeShared(FrameT _frame, ReferenceT _reference, - ReferenceUnits _units=RADIANS, bool _ignoreTimeStep = false); + ReferenceUnits _units = ReferenceUnits::RADIANS, + bool _ignoreTimeStep = false); /// \brief Environmental data frame. FrameT frame; diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index 3ba360e967..2a5a1d6855 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -78,8 +78,10 @@ class EnvironmentLoaderPrivate /// \brief Map of supported spatial units. public: const QMap unitMap{ - {QString("degree"), components::EnvironmentalData::DEGREES}, - {QString("radians"), components::EnvironmentalData::RADIANS} + {QString("degree"), + components::EnvironmentalData::ReferenceUnits::DEGREES}, + {QString("radians"), + components::EnvironmentalData::ReferenceUnits::RADIANS} }; /// \brief Spatial reference.