From 1d3235a214c81b541b89dfa7d5b9142e7b6a27e4 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Wed, 24 Aug 2022 14:21:37 -0700 Subject: [PATCH 1/2] Acoustic comms plugin (#1608) * Added acoustic comms plugin Signed-off-by: Aditya --- examples/worlds/acoustic_comms.sdf | 254 ++++++++++++++++++ .../worlds/acoustic_comms_moving_targets.sdf | 250 +++++++++++++++++ src/systems/CMakeLists.txt | 1 + src/systems/acoustic_comms/AcousticComms.cc | 211 +++++++++++++++ src/systems/acoustic_comms/AcousticComms.hh | 86 ++++++ src/systems/acoustic_comms/CMakeLists.txt | 6 + test/integration/CMakeLists.txt | 1 + test/integration/acoustic_comms.cc | 157 +++++++++++ 8 files changed, 966 insertions(+) create mode 100644 examples/worlds/acoustic_comms.sdf create mode 100644 examples/worlds/acoustic_comms_moving_targets.sdf create mode 100644 src/systems/acoustic_comms/AcousticComms.cc create mode 100644 src/systems/acoustic_comms/AcousticComms.hh create mode 100644 src/systems/acoustic_comms/CMakeLists.txt create mode 100644 test/integration/acoustic_comms.cc diff --git a/examples/worlds/acoustic_comms.sdf b/examples/worlds/acoustic_comms.sdf new file mode 100644 index 0000000000..6b20c698fb --- /dev/null +++ b/examples/worlds/acoustic_comms.sdf @@ -0,0 +1,254 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + 6 + 1400 + + + + 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 + + + + 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 + + + + + + + 2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr1
+ addr1/rx + + + + + -2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + +
addr2
+ addr2/rx + + /broker/bind + /broker/unbind + + + + + + + 5 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr3
+ addr3/rx + + + + + -5 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr4
+ addr4/rx + + + + + diff --git a/examples/worlds/acoustic_comms_moving_targets.sdf b/examples/worlds/acoustic_comms_moving_targets.sdf new file mode 100644 index 0000000000..1a474f1cbf --- /dev/null +++ b/examples/worlds/acoustic_comms_moving_targets.sdf @@ -0,0 +1,250 @@ + + + + + 0 1 -10 + + + 0.001 + 1.0 + + + + + + + + + 1400 + + + + 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 + + + + 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 + + + + + + + 2 0 1 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + + + + + + + + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr1
+ addr1/rx + + + + + -2 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + +
addr2
+ addr2/rx + + /broker/bind + /broker/unbind + + + + + + + 5 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr3
+ addr3/rx + + + + + -5 0 1 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + + + + + + + + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + +
addr4
+ addr4/rx + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 57e8050a75..6c0f5b402b 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -96,6 +96,7 @@ function(gz_add_system system_name) endfunction() add_subdirectory(ackermann_steering) +add_subdirectory(acoustic_comms) add_subdirectory(air_pressure) add_subdirectory(altimeter) add_subdirectory(apply_joint_force) diff --git a/src/systems/acoustic_comms/AcousticComms.cc b/src/systems/acoustic_comms/AcousticComms.cc new file mode 100644 index 0000000000..bab0c84781 --- /dev/null +++ b/src/systems/acoustic_comms/AcousticComms.cc @@ -0,0 +1,211 @@ +/* + * 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. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ +#include + +#include +#include +#include +#include "gz/sim/comms/Broker.hh" +#include "gz/sim/comms/MsgManager.hh" +#include "gz/sim/Util.hh" +#include "AcousticComms.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private Acoustic comms data class. +class AcousticComms::Implementation +{ + /// \brief Default max range for acoustic comms in metres. + public: double maxRange = 1000.0; + + /// \brief Default speed of sound in air in metres/sec. + public: double speedOfSound = 343.0; + + /// \brief Position of the transmitter at the time the message was + /// sent, or first processed. + public: std::unordered_map + , math::Vector3d> + poseSrcAtMsgTimestamp; +}; + +////////////////////////////////////////////////// +AcousticComms::AcousticComms() + : dataPtr(gz::utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void AcousticComms::Load( + const Entity &_entity, + std::shared_ptr _sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + if (_sdf->HasElement("max_range")) + { + this->dataPtr->maxRange = _sdf->Get("max_range"); + } + if (_sdf->HasElement("speed_of_sound")) + { + this->dataPtr->speedOfSound = _sdf->Get("speed_of_sound"); + } + + gzmsg << "AcousticComms configured with max range : " << + this->dataPtr->maxRange << " m and speed of sound : " << + this->dataPtr->speedOfSound << " m/s." << std::endl; +} + +////////////////////////////////////////////////// +void AcousticComms::Step( + const UpdateInfo &_info, + const comms::Registry &_currentRegistry, + comms::Registry &_newRegistry, + EntityComponentManager &_ecm) +{ + // Initialize entity if needed. + for (auto & [address, content] : _currentRegistry) + { + if (content.entity == kNullEntity) + { + auto entities = entitiesFromScopedName(content.modelName, _ecm); + if (entities.empty()) + continue; + + auto entityId = *(entities.begin()); + if (entityId == kNullEntity) + continue; + + _newRegistry[address].entity = entityId; + } + } + + for (auto & [address, content] : _currentRegistry) + { + // Reference to the outbound queue for this address. + auto &outbound = content.outboundMsgs; + + // Is the source address bound? + auto itSrc = _currentRegistry.find(address); + bool srcAddressBound = itSrc != _currentRegistry.end(); + + // Is the source address attached to a model? + bool srcAddressAttachedToModel = + srcAddressBound && itSrc->second.entity != kNullEntity; + + comms::DataQueue newOutbound; + + if (srcAddressAttachedToModel) + { + // All these messages need to be processed. + for (auto &msg : outbound) + { + // Is the destination address bound? + auto itDst = _currentRegistry.find(msg->dst_address()); + bool dstAddressBound = itDst != _currentRegistry.end(); + + // Is the destination address attached to a model? + bool dstAddressAttachedToModel = + dstAddressBound && itDst->second.entity != kNullEntity; + + if (!dstAddressAttachedToModel) + continue; + + // The plugin checks the distance travelled by the signal + // so far. If it is more than the maxRange, it is dropped + // and would never reach the destination. + // If it has already reached the destination but not as far + // as maxRange, it is processed. + // If it has reached neither the destination nor the maxRange, + // it is considered in transit. + + if (this->dataPtr->poseSrcAtMsgTimestamp.count(msg) == 0) + { + // This message is being processed for the first time. + // Record the current position of the sender and use it + // for distance calculations. + this->dataPtr->poseSrcAtMsgTimestamp[msg] = + worldPose(itSrc->second.entity, _ecm).Pos(); + } + + const auto& poseSrc = + this->dataPtr->poseSrcAtMsgTimestamp[msg]; + + // Calculate distance between the bodies. + const auto poseDst = worldPose(itDst->second.entity, _ecm).Pos(); + const auto distanceToTransmitter = (poseSrc - poseDst).Length(); + + // Calculate distance covered by the message. + const std::chrono::steady_clock::time_point currTime(_info.simTime); + const auto timeOfTransmission = msg->mutable_header()->stamp(); + + const auto currTimestamp = + std::chrono::nanoseconds(currTime.time_since_epoch()); + const auto packetTimestamp = + std::chrono::seconds(timeOfTransmission.sec()) + + std::chrono::nanoseconds(timeOfTransmission.nsec()); + + const std::chrono::duration deltaT = + currTimestamp - packetTimestamp; + const double distanceCoveredByMessage = deltaT.count() * + this->dataPtr->speedOfSound; + + // Check the msgs that haven't exceeded the maxRange. + if (distanceCoveredByMessage <= this->dataPtr->maxRange) + { + if (distanceCoveredByMessage >= distanceToTransmitter) + { + // This message needs to be processed. + _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + this->dataPtr->poseSrcAtMsgTimestamp.erase(msg); + } + else + { + // This message is still in transit, should be kept in the + // outbound buffer of source and not moved to inbound of + // the destination. + newOutbound.push_back(msg); + } + } + else + { + // This message exceeded the maxRange. + // Stop keeping track of the position of its source. + this->dataPtr->poseSrcAtMsgTimestamp.erase(msg); + } + } + } + + // Clear the outbound queue, except for messages that were + // in transit. + _newRegistry[address].outboundMsgs = newOutbound; + } +} + +GZ_ADD_PLUGIN(AcousticComms, + System, + comms::ICommsModel::ISystemConfigure, + comms::ICommsModel::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(AcousticComms, + "gz::sim::systems::AcousticComms") diff --git a/src/systems/acoustic_comms/AcousticComms.hh b/src/systems/acoustic_comms/AcousticComms.hh new file mode 100644 index 0000000000..a4e6adedd3 --- /dev/null +++ b/src/systems/acoustic_comms/AcousticComms.hh @@ -0,0 +1,86 @@ +/* + * 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. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ +#ifndef GZ_SIM_SYSTEMS_ACOUSTICCOMMS_HH_ +#define GZ_SIM_SYSTEMS_ACOUSTICCOMMS_HH_ + +#include + +#include +#include "gz/sim/comms/ICommsModel.hh" +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + /// \brief A comms model that simulates communication using acoustic + /// devices. The model uses simple distance based acoustics model. + /// + /// This system can be configured with the following SDF parameters: + /// + /// * Optional parameters: + /// * : Hard limit on range (meters). No communication will + /// happen beyond this range. Default is 1000. + /// * : Speed of sound in the medium (meters/sec). + /// Default is 343.0 + /// + /// Here's an example: + /// + /// 6 + /// 1400 + /// + + class AcousticComms: + public gz::sim::comms::ICommsModel + { + public: explicit AcousticComms(); + + // Documentation inherited. + public: void Load(const gz::sim::Entity &_entity, + std::shared_ptr _sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + // Documentation inherited. + public: void Step(const gz::sim::UpdateInfo &_info, + const gz::sim::comms::Registry &_currentRegistry, + gz::sim::comms::Registry &_newRegistry, + gz::sim::EntityComponentManager &_ecm) override; + + // Impl pointer + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +} +} +} + +#endif diff --git a/src/systems/acoustic_comms/CMakeLists.txt b/src/systems/acoustic_comms/CMakeLists.txt new file mode 100644 index 0000000000..537f4e2551 --- /dev/null +++ b/src/systems/acoustic_comms/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(acoustic-comms + SOURCES + AcousticComms.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} +) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2d5babb5ae..b2c99f0fee 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -2,6 +2,7 @@ set(TEST_TYPE "INTEGRATION") set(tests ackermann_steering_system.cc + acoustic_comms.cc air_pressure_system.cc altimeter_system.cc apply_joint_force_system.cc diff --git a/test/integration/acoustic_comms.cc b/test/integration/acoustic_comms.cc new file mode 100644 index 0000000000..f7ea59db3b --- /dev/null +++ b/test/integration/acoustic_comms.cc @@ -0,0 +1,157 @@ +/* + * 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 +#include +#include + +#include +#include +#include +#include "gz/sim/Server.hh" +#include "test_config.hh" // NOLINT(build/include) +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +class AcousticCommsTestDefinition +{ +public: + std::string worldFile; + std::string srcAddr; + std::string dstAddr; + int numMsgs; + + AcousticCommsTestDefinition( + std::string worldFile_, std::string srcAddr_, + std::string dstAddr_, int numMsgs_) : + worldFile(worldFile_), srcAddr(srcAddr_), + dstAddr(dstAddr_), numMsgs(numMsgs_) {} +}; + +///////////////////////////////////////////////// +class AcousticCommsTestFixture : + public ::testing::TestWithParam +{ +}; + +TEST_P(AcousticCommsTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AcousticCommsTestTemplate)) +{ + auto param = GetParam(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = + gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", param.worldFile); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + size_t iters = 1000; + server.Run(true, iters, false); + + int msgCounter = 0; + std::mutex mutex; + auto cb = [&](const msgs::Dataframe &_msg) -> void + { + // Verify msg content + std::lock_guard lock(mutex); + std::string expected = "hello world " + std::to_string(msgCounter); + + gz::msgs::StringMsg receivedMsg; + receivedMsg.ParseFromString(_msg.data()); + EXPECT_EQ(expected, receivedMsg.data()); + msgCounter++; + }; + + // Create subscriber. + gz::transport::Node node; + std::string addr = param.dstAddr; + std::string subscriptionTopic = param.dstAddr + "/rx"; + + // Subscribe to a topic by registering a callback. + auto cbFunc = std::function(cb); + EXPECT_TRUE(node.Subscribe(subscriptionTopic, cbFunc)) + << "Error subscribing to topic [" << subscriptionTopic << "]"; + + // Create publisher. + std::string publicationTopic = "/broker/msgs"; + auto pub = node.Advertise(publicationTopic); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Prepare the message. + gz::msgs::Dataframe msg; + msg.set_src_address(param.srcAddr); + msg.set_dst_address(addr); + + // Publish some messages. + gz::msgs::StringMsg payload; + int pubCount = param.numMsgs; + for (int i = 0; i < pubCount; ++i) + { + // Prepare the payload. + payload.set_data("hello world " + std::to_string(i)); + std::string serializedData; + EXPECT_TRUE(payload.SerializeToString(&serializedData)) + << payload.DebugString(); + msg.set_data(serializedData); + EXPECT_TRUE(pub.Publish(msg)); + server.Run(true, 100, false); + } + + // Verify subscriber received all msgs. + bool done = false; + for (int sleep = 0; !done && sleep < 3; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + { + std::lock_guard lock(mutex); + done = (msgCounter == pubCount) && (pubCount != 0); + } + } + if (param.numMsgs != 0) + { + EXPECT_TRUE(done); + } + EXPECT_EQ(pubCount, msgCounter); +} + +INSTANTIATE_TEST_SUITE_P( + AcousticCommsTests, + AcousticCommsTestFixture, + ::testing::Values( + AcousticCommsTestDefinition( + "acoustic_comms.sdf", "addr2", "addr1", 3), + AcousticCommsTestDefinition( + "acoustic_comms.sdf", "addr4", "addr3", 0), + // The source is moving and the destination is stationary. + AcousticCommsTestDefinition( + "acoustic_comms_moving_targets.sdf", "addr2", "addr1", 3), + // The source is stationary and the destnation is moving. + AcousticCommsTestDefinition( + "acoustic_comms_moving_targets.sdf", "addr4", "addr3", 3) + ) +); From 5c307e001274e808d5b1478d8ffdbe61b3736afe Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Tue, 13 Sep 2022 10:30:40 -0700 Subject: [PATCH 2/2] 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 + + + + + + +