Updated comms model

subt-communication/subt_communication_model/src/subt_communication_model.cpp

@@ -75,8 +75,7 @@ attempt_send(const radio_configuration& radio,
   tx_state.bytes_sent.push_back(std::make_pair(now, num_bytes));
   tx_state.bytes_sent_this_epoch += num_bytes;
 
-  // Get the received power based on TX power and position of each
-  // node
+  // Get the received power based on TX power and position of each node
   auto rx_power_dist = radio.pathloss_f(radio.default_tx_power,
                                         tx_state,
                                         rx_state);

subt-communication/subt_rf_interface/include/subt_rf_interface/subt_rf_model.h

@@ -37,11 +37,15 @@ struct rf_configuration
   double fading_exponent;  ///< Fading exponent
   double L0;               ///< Received power at 1m (in dBm)
   double sigma;            ///< Standard deviation for received power
+  double scaling_factor;   ///< Scaling factor
+  double range_per_hop;    ///< Extra ranged added per breadcrumb
   rf_configuration() :
       max_range(50.0),
       fading_exponent(2.5),
       L0(40),
-      sigma(10)
+      sigma(10),
+      scaling_factor(1.0),
+      range_per_hop(2.0)
   { }
 
   /// Output stream operator
@@ -54,7 +58,9 @@ struct rf_configuration
         << "-- max_range: " << config.max_range << std::endl
         << "-- fading_exponent: " << config.fading_exponent << std::endl
         << "-- L0: " << config.L0 << std::endl
-        << "-- sigma: " << config.sigma << std::endl;
+        << "-- sigma: " << config.sigma << std::endl
+        << "-- scaling_factor: " << config.scaling_factor << std::endl
+        << "-- range_per_hop: " << config.range_per_hop << std::endl;
 
     return oss;
   }
@@ -64,7 +70,7 @@ struct rf_configuration
 ///
 /// Compute the pathloss based on distance between two nodes and
 /// return the received power.
-/// 
+///
 /// @param tx_power Transmit power (dBm)
 /// @param tx_state Transmit state (pose)
 /// @param rx_state Receiver state (pose)
@@ -79,7 +85,7 @@ rf_power distance_based_received_power(const double& tx_power,
 /// Compute the pathloss based on distance between two nodes and
 /// return the received power. Vary return by drawing from normal
 /// distribution.
-/// 
+///
 /// @param tx_power Transmit power (dBm)
 /// @param tx_state Transmit state (pose)
 /// @param rx_state Receiver state (pose)
@@ -88,8 +94,33 @@ rf_power log_normal_received_power(const double& tx_power,
                                    radio_state& tx_state,
                                    radio_state& rx_state,
                                    const rf_configuration& config);
+
+/// Compute received power based on distance.
+///
+/// Compute the pathloss based on distance between two nodes and
+/// return the received power. Vary return by drawing from normal
+/// distribution.
+///
+/// @param tx_power Transmit power (dBm)
+/// @param range Greatest distance in a single hop (m)
+/// @param num_hops Number of breadcrumbs crossed
+/// @param config Physical-layer configuration
+rf_power log_normal_v2_received_power(const double& tx_power,
+                                      const double& range,
+                                      const unsigned int& num_hops,
+                                      const rf_configuration& config);
+
+/// Compute received power based on visibility information only.
+///
+/// Compute the pathloss based on the visibility cost between two nodes and
+/// return the received power.Vary return by drawing from normal
+/// distribution.
+///
+/// @param tx_power Transmit power (dBm)
+/// @param config Physical-layer configuration
+rf_power visibility_only_received_power(const double& tx_power,
+                                        const rf_configuration& config);
 }
 }
 }
 #endif
-

subt-communication/subt_rf_interface/src/subt_rf_model.cpp

@@ -56,7 +56,35 @@ rf_power log_normal_received_power(const double& tx_power,
   }
 
   double PL = config.L0 + 10 * config.fading_exponent * log10(range);
-
+
+  return {tx_power - PL, config.sigma};
+}
+
+/////////////////////////////////////////////
+rf_power log_normal_v2_received_power(const double& tx_power,
+                                      const double& range,
+                                      const unsigned int& num_hops,
+                                      const rf_configuration& config)
+{
+  if(config.max_range > 0.0 &&
+     range > config.max_range) {
+    return {-std::numeric_limits<double>::infinity(), 0.0};
+  }
+
+  double adjusted_range = config.scaling_factor *
+    (range + num_hops * config.range_per_hop);
+
+  double PL = config.L0 + 10 * config.fading_exponent * log10(adjusted_range);
+
+  return {tx_power - PL, config.sigma};
+}
+
+/////////////////////////////////////////////
+rf_power visibility_only_received_power(const double& tx_power,
+                                        const rf_configuration& config)
+{
+  double PL = config.L0 + 10 * config.fading_exponent;
+
   return {tx_power - PL, config.sigma};
 }
 

subt_ign/CMakeLists.txt

@@ -178,6 +178,18 @@ install(TARGETS validate_connections
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
+add_executable(validate_visibility_table
+  src/VisibilityTable.cc
+  src/SimpleDOTParser.cc)
+target_link_libraries(validate_visibility_table
+  PRIVATE
+    ignition-gazebo${IGN_GAZEBO_VER}::core
+    ${catkin_LIBRARIES}
+)
+install(TARGETS validate_visibility_table
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
 
 # Create the libCommsBrokerPlugin.so library.
 set(comms_broker_plugin_name CommsBrokerPlugin)

subt_ign/include/subt_ign/CommonTypes.hh

@@ -22,6 +22,7 @@
 #include <string>
 #include <utility>
 #include <ignition/math/graph/Graph.hh>
+#include <subt_ign/VisibilityTypes.hh>
 
 namespace subt
 {
@@ -41,7 +42,7 @@ namespace subt
   using VisibilityInfo =
     std::map<std::pair<ignition::math::graph::VertexId,
                        ignition::math::graph::VertexId>,
-             double>;
+             VisibilityCost>;
 
   /// \brief Class used to store information about a member of the team.
   class TeamMember

subt_ign/include/subt_ign/VisibilityTable.hh

@@ -68,8 +68,8 @@ namespace subt
     /// \param[in] _from A 3D position.
     /// \param[in] _to A 3D position.
     /// \return The visibility cost from one point to the other.
-    public: double Cost(const ignition::math::Vector3d &_from,
-                        const ignition::math::Vector3d &_to) const;
+    public: VisibilityCost Cost(const ignition::math::Vector3d &_from,
+                                const ignition::math::Vector3d &_to) const;
 
     /// \brief Generate a binary .dat file containing a list of sample points
     /// that are within the explorable areas of the world. Each sample point
@@ -91,6 +91,11 @@ namespace subt
     public: const std::map<std::tuple<int32_t, int32_t, int32_t>, uint64_t>
       &Vertices() const;
 
+    /// \brief Get the collection of breadcrumbs and their locations.
+    /// \return the collection.
+    public: const std::map<uint64_t, std::vector<ignition::math::Vector3d>>
+      &Breadcrumbs() const;
+
     /// \brief Populate the visibility information in memory.
     /// \param[in] _relays Set of vertices containing breadcrumb robots.
     /// You should call this function when the breadcrumbs are updated.
@@ -111,6 +116,10 @@ namespace subt
     public: void PopulateVisibilityInfo(
                       const std::set<ignition::math::Vector3d> &_relayPoses);
 
+    /// \brief Print the visibility table containing all combination of pairs.
+    /// \param[in] _info The visibility table to print.
+    public: void PrintAll(const VisibilityInfo &_info) const;
+
     /// \brief Load a look up table from a file. It will try to load a file
     /// located in the same directory as the world file, with the same world
     /// name but with extension .dat.
@@ -161,6 +170,17 @@ namespace subt
     /// \brief Generate the visibility LUT in disk.
     private: void WriteOutputFile();
 
+    /// \brief Calculate the greatest distance between a pair of breadcrumbs.
+    /// \param[in] _visibilityInfoWithRelays The visibility information known
+    /// at the moment.
+    /// \param[in] _relaySequence The sequence of breadcrumbs to traverse.
+    /// \param[in] _to Destination tile.
+    /// \return The maximum distance.
+    private: double MaxDistanceSingleHop(
+      const VisibilityInfo &_visibilityInfoWithRelays,
+      const std::vector<ignition::math::graph::VertexId> &_relaySequence,
+      const ignition::math::graph::VertexId &_to) const;
+
     /// \brief The graph modeling the connectivity.
     private: VisibilityGraph visibilityGraph;
 
@@ -194,6 +214,12 @@ namespace subt
 
     /// \brief The world name.
     private: std::string worldName;
+
+    /// \brief The map of breadcrumbs. The key is the tile Id where breadcrumbs
+    /// are located. The value is the vector of breadcrumb positions within that
+    /// tile.
+    private: std::map<uint64_t, std::vector<ignition::math::Vector3d>>
+      breadcrumbs;
   };
 }
 

subt_ign/include/subt_ign/VisibilityTypes.hh

@@ -20,10 +20,39 @@
 #include <map>
 #include <string>
 #include <utility>
+#include <vector>
 #include <ignition/math/graph/Graph.hh>
+#include <ignition/math/Vector3.hh>
 
 namespace subt
 {
+
+/// \brief A class that contains some information about the cost of
+/// communicating two robots.
+class VisibilityCost
+{
+  /// \brief The cost from one tile to another tile.
+  public: double cost;
+
+  /// \brief The best route connecting source and destination. No that route
+  /// only contains the sequence of breadcrumbs.
+  public: std::vector<ignition::math::graph::VertexId> route;
+
+  /// \brief The position of the first breadcrumb in the route. This value
+  /// should be ignored if route is empty (no breadcrumbs).
+  public: ignition::math::Vector3d posFirstBreadcrumb;
+
+  /// \brief The position of the last breadcrumb in the route. This value
+  /// should be ignored if route is empty (no breadcrumbs).
+  public: ignition::math::Vector3d posLastBreadcrumb;
+
+  /// \brief A path from source to destination can traverse 0-N breadcrumbs.
+  /// This field stores the maximum distance between a pair of breadcrumbs.
+  /// E.g.: source->A->B->C->destination. Let's assume the next
+  /// Euclidean distances: dist(A, B): 2; dist(B, C): 3. This field stores a 3.
+  public: double greatestDistanceSingleHop;
+};
+
 /// \def VisibilityGraph
 /// \brief An undirected graph to represent communication visibility between
 /// different areas of the world.
@@ -36,6 +65,6 @@ using VisibilityGraph =
 using VisibilityInfo =
     std::map<std::pair<ignition::math::graph::VertexId,
                        ignition::math::graph::VertexId>,
-             double>;
+             VisibilityCost>;
 }
 #endif

subt_ign/launch/cave_circuit.ign

@@ -132,7 +132,7 @@
     "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416),
     "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416)
   }
-  
+
   allExecutables = ""
 
   # key: parent, value: child
@@ -602,6 +602,8 @@
         <fading_exponent><%= $commsFadingExponent %></fading_exponent>
         <L0>40</L0>
         <sigma>10.0</sigma>
+        <scaling_factor>0.55</scaling_factor>
+        <range_per_hop>2.0</range_per_hop>
       </range_config>
 
       <visibility_config>

subt_ign/src/CommsBrokerPlugin.cc

@@ -87,6 +87,14 @@ bool CommsBrokerPlugin::Load(const tinyxml2::XMLElement *_elem)
       if (elem)
         rangeConfig.sigma = std::stod(elem->GetText());
 
+      elem = rfConfigElem->FirstChildElement("scaling_factor");
+      if (elem)
+        rangeConfig.scaling_factor = std::stod(elem->GetText());
+
+      elem = rfConfigElem->FirstChildElement("range_per_hop");
+      if (elem)
+        rangeConfig.range_per_hop = std::stod(elem->GetText());
+
       igndbg << "Loading range_config from SDF: \n" << rangeConfig << std::endl;
     }
 
@@ -324,7 +332,7 @@ void CommsBrokerPlugin::UpdateIfNewBreadcrumbs()
   if (newBreadcrumbFound)
   {
     std::set<ignition::math::Vector3d> breadcrumbPoses;
-    for (const auto& it: this->breadcrumbs)
+    for (const auto& it : this->breadcrumbs)
       breadcrumbPoses.insert(it.second.Pos());
     this->visibilityModel->PopulateVisibilityInfo(breadcrumbPoses);
     ignmsg << "New breadcrumb detected, visibility graph updated" << std::endl;