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 305bb0fd44..a80168a752 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 8a0755a602..72c197a0a8 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)
+ )
+);