Skip to content

Commit

Permalink
Acoustic comms : Packet collision timeout (#1755)
Browse files Browse the repository at this point in the history 
* Acoustic comms : Packet collision timeouts added.

Signed-off-by: Aditya <[email protected]>
  • Loading branch information
@adityapande-1995
adityapande-1995 authored Nov 4, 2022
1 parent c4cd2c0 commit d9bfacb
Show file tree
Hide file tree
Showing 4 changed files with 364 additions and 15 deletions.
256 changes: 256 additions & 0 deletions examples/worlds/acoustic_comms_packet_collision.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,256 @@
<?xml version="1.0" ?>
<!--
Gazebo acoustic-comms plugin demo.
-->
<sdf version="1.6">
<world name="acoustic_comms">

<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-acoustic-comms-system"
name="gz::sim::systems::AcousticComms">
<max_range>6</max_range>
<speed_of_sound>1400</speed_of_sound>
<collision_time_per_byte>1</collision_time_per_byte>
<collision_time_packet_drop>0.1</collision_time_packet_drop>
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="box1">
<pose>2 0 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>0.16666</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16666</iyy>
<iyz>0</iyz>
<izz>0.16666</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<plugin
filename="gz-sim-comms-endpoint-system"
name="gz::sim::systems::CommsEndpoint">
<address>addr1</address>
<topic>addr1/rx</topic>
</plugin>
</model>

<model name="box2">
<pose>-2 0 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>0.16666</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16666</iyy>
<iyz>0</iyz>
<izz>0.16666</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>0 0 1 1</specular>
</material>
</visual>
</link>

<plugin
filename="gz-sim-comms-endpoint-system"
name="gz::sim::systems::CommsEndpoint">
<address>addr2</address>
<topic>addr2/rx</topic>
<broker>
<bind_service>/broker/bind</bind_service>
<unbind_service>/broker/unbind</unbind_service>
</broker>
</plugin>

</model>

<model name="box3">
<pose>5 0 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>0.16666</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16666</iyy>
<iyz>0</iyz>
<izz>0.16666</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<plugin
filename="gz-sim-comms-endpoint-system"
name="gz::sim::systems::CommsEndpoint">
<address>addr3</address>
<topic>addr3/rx</topic>
</plugin>
</model>

<model name="box4">
<pose>-5 0 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>0.16666</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16666</iyy>
<iyz>0</iyz>
<izz>0.16666</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<plugin
filename="gz-sim-comms-endpoint-system"
name="gz::sim::systems::CommsEndpoint">
<address>addr4</address>
<topic>addr4/rx</topic>
</plugin>
</model>

</world>
</sdf>
76 changes: 74 additions & 2 deletions src/systems/acoustic_comms/AcousticComms.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
* Development of this module has been funded by the Monterey Bay Aquarium
* Research Institute (MBARI) and the David and Lucile Packard Foundation
*/
#include <unordered_map>

#include <gz/common/Profiler.hh>
#include <gz/plugin/Register.hh>
Expand All @@ -42,11 +41,26 @@ class AcousticComms::Implementation
/// \brief Default speed of sound in air in metres/sec.
public: double speedOfSound = 343.0;

/// \brief Default collision time interval per byte in sec.
public: double collisionTimePerByte = 0;

/// \brief Default collision time when packet is dropped
/// in sec.
public: double collisionTimePacketDrop = 0;

/// \brief Position of the transmitter at the time the message was
/// sent, or first processed.
public: std::unordered_map
<std::shared_ptr<msgs::Dataframe>, math::Vector3d>
poseSrcAtMsgTimestamp;

/// \brief Map that holds data of the address of a receiver,
/// the timestamp, length of the last message recevied by it.
public: std::unordered_map
<std::string,
std::tuple<std::chrono::duration<double>,
std::chrono::duration<double>>>
lastMsgReceivedInfo;
};

//////////////////////////////////////////////////
Expand All @@ -70,6 +84,11 @@ void AcousticComms::Load(
{
this->dataPtr->speedOfSound = _sdf->Get<double>("speed_of_sound");
}
if (_sdf->HasElement("collision_time_per_byte"))
{
this->dataPtr->collisionTimePerByte =
_sdf->Get<double>("collision_time_per_byte");
}

gzmsg << "AcousticComms configured with max range : " <<
this->dataPtr->maxRange << " m and speed of sound : " <<
Expand Down Expand Up @@ -175,8 +194,61 @@ void AcousticComms::Step(
{
if (distanceCoveredByMessage >= distanceToTransmitter)
{
// This message has effectively reached the destination.
bool receivedSuccessfully = false;

// Check for time collision
if (this->dataPtr->lastMsgReceivedInfo.count(
msg->dst_address()) == 0)
{
// This is the first message received by this address.
receivedSuccessfully = true;
}
else
{
// A previous msg was already received at this address.
// time gap = current time - time at which last msg was received.
std::chrono::duration<double> timeGap = currTimestamp -
std::get<0>(this->dataPtr->lastMsgReceivedInfo[
msg->dst_address()]);

// drop interval = collision time interval per byte *
// length of last msg received.
auto dropInterval = std::chrono::duration<double>(
std::get<1>(this->dataPtr->lastMsgReceivedInfo[
msg->dst_address()]));

if (timeGap >= dropInterval)
receivedSuccessfully = true;
}

// This message needs to be processed.
_newRegistry[msg->dst_address()].inboundMsgs.push_back(msg);
// Push the msg to inbound of the destination if
// receivedSuccessfully is true, else it is dropped.
if (receivedSuccessfully)
{
_newRegistry[msg->dst_address()].inboundMsgs.push_back(msg);
// Update the (receive time, length of msg) tuple
// for the last msg for this address.
auto blockingTime = std::chrono::duration<double>(
this->dataPtr->collisionTimePerByte *
msg->data().length());

this->dataPtr->lastMsgReceivedInfo[msg->dst_address()] =
std::make_tuple(currTimestamp, blockingTime);
}
else
{
// Packet was dropped due to collision.
auto blockingTime = std::chrono::duration<double>(
this->dataPtr->collisionTimePacketDrop
);

std::get<1>(this->dataPtr->lastMsgReceivedInfo[
msg->dst_address()]) += blockingTime;
}

// Stop keeping track of the position of its source.
this->dataPtr->poseSrcAtMsgTimestamp.erase(msg);
}
else
Expand Down
12 changes: 12 additions & 0 deletions src/systems/acoustic_comms/AcousticComms.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,9 @@
#define GZ_SIM_SYSTEMS_ACOUSTICCOMMS_HH_

#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>

#include <sdf/sdf.hh>
#include "gz/sim/comms/ICommsModel.hh"
Expand All @@ -49,13 +52,22 @@ namespace systems
/// happen beyond this range. Default is 1000.
/// * <speed_of_sound>: Speed of sound in the medium (meters/sec).
/// Default is 343.0
/// * <collision_time_per_byte> : If a subscriber receives a message
/// 'b' bytes long at time 't0', it won't receive
/// and other message till time :
/// 't0 + b * collision_time_per_byte'
/// * <collision_time_packet_drop> : If a packet is dropped at time
/// `t0`, the next packet won't be received until
/// time `t0 + collision_time_packet_drop`
///
/// Here's an example:
/// <plugin
/// filename="gz-sim-acoustic-comms-system"
/// name="gz::sim::systems::AcousticComms">
/// <max_range>6</max_range>
/// <speed_of_sound>1400</speed_of_sound>
/// <collision_time_per_byte>0.001</collision_time_per_byte>
/// <collision_time_packet_drop>0.001</collision_time_packet_drop>
/// </plugin>

class AcousticComms:
Expand Down
Loading

0 comments on commit d9bfacb

Please sign in to comment.