Do not update neighbor information when the message goes through breadcrumbs. (#970)

Signed-off-by: Carlos Agüero <[email protected]>

Author: caguero

subt-communication/subt_communication_broker/src/subt_communication_broker.cpp

@@ -216,14 +216,18 @@ void Broker::DispatchMessages()
 
         bool sendPacket;
         double rssi;
-        std::tie(sendPacket, rssi) =
+        bool usingBreadcrumbs;
+        std::tie(sendPacket, rssi, usingBreadcrumbs) =
           communication_function(txNode->second->radio,
                                  txNode->second->rf_state,
                                  rxNode->second->rf_state,
                                  msg.data().size());
 
         if (sendPacket)
         {
+          // When using a breadcrumb we don't include the rssi
+          if (usingBreadcrumbs)
+            rssi = std::numeric_limits<double>::lowest();
           msg.set_rssi(rssi);
 
           if (!this->node.Request(client.address, msg))

subt-communication/subt_communication_broker/src/subt_communication_client.cpp

@@ -452,8 +452,11 @@ bool CommsClient::OnMessageRos(subt_msgs::DatagramRos::Request &_req,
 
   std::lock_guard<std::mutex> lock(this->mutex);
 
-  this->neighbors[_req.src_address] =
-      std::make_pair(ros::Time::now().toSec(), _req.rssi);
+  if (_req.rssi > std::numeric_limits<double>::lowest())
+  {
+    this->neighbors[_req.src_address] =
+        std::make_pair(ros::Time::now().toSec(), _req.rssi);
+  }
 
   for (auto cb : this->callbacks)
   {

subt-communication/subt_communication_model/include/subt_communication_model/subt_communication_model.h

@@ -75,17 +75,18 @@ inline std::ostream& operator<<(std::ostream& oss,
 /// @param tx_state Current state of the transmitter (pose)
 /// @param rx_state Current state of the receiver (pose)
 /// @param num_bytes Size of the packet
-/// @return std::tuple<bool, double> reporting if the packet should be
-/// delivered and the received signal strength (in dBm)
-std::tuple<bool, double>
+/// @return std::tuple<bool, double, bool> reporting if the packet should be
+/// delivered, the received signal strength (in dBm) and if breadcrumbs were
+/// used.
+std::tuple<bool, double, bool>
 attempt_send(const radio_configuration& radio,
              rf_interface::radio_state& tx_state,
              rf_interface::radio_state& rx_state,
              const uint64_t& num_bytes
              );
 
 /// Function signature for the communication model.
-typedef std::function<std::tuple<bool, double>(const radio_configuration&,
+typedef std::function<std::tuple<bool, double, bool>(const radio_configuration&,
                            rf_interface::radio_state&,
                            rf_interface::radio_state&,
                            const uint64_t&)> communication_function;

subt-communication/subt_communication_model/src/subt_communication_model.cpp

@@ -34,7 +34,7 @@ inline double dbmToPow(double x) { return 0.001 * pow(10., x / 10.); }
 inline double QPSKPowerToBER(double P, double N) { return erfc(sqrt(P / N)); }
 
 /////////////////////////////////////////////
-std::tuple<bool, double>
+std::tuple<bool, double, bool>
 attempt_send(const radio_configuration& radio,
              rf_interface::radio_state& tx_state,
              rf_interface::radio_state& rx_state,
@@ -68,17 +68,19 @@ attempt_send(const radio_configuration& radio,
   {
     // ignwarn << "Bitrate limited: " << bits_sent << "bits sent (limit: "
     // << radio.capacity  * epoch_duration.toSec() << std::endl;
-    return std::make_tuple(false, std::numeric_limits<double>::lowest());
+    return std::make_tuple(false, std::numeric_limits<double>::lowest(), false);
   }
 
   // Record these bytes
   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
+  bool usingBreadcrumbs;
   auto rx_power_dist = radio.pathloss_f(radio.default_tx_power,
                                         tx_state,
-                                        rx_state);
+                                        rx_state,
+                                        usingBreadcrumbs);
 
   double rx_power = rx_power_dist.mean;
   if(rx_power_dist.variance > 0.0) {
@@ -113,7 +115,7 @@ attempt_send(const radio_configuration& radio,
   bool packet_received = rand_draw > packet_drop_prob;
 
   if(!packet_received)
-    return std::make_tuple(false, std::numeric_limits<double>::lowest());
+    return std::make_tuple(false, std::numeric_limits<double>::lowest(), false);
 
   // Maintain running window of bytes received over the last epoch, e.g.,
   // 1s
@@ -140,14 +142,14 @@ attempt_send(const radio_configuration& radio,
     // ignwarn < <"Bitrate limited: " <<  bits_received
     // << "bits received (limit: " << radio.capacity * epoch_duration.toSec()
     // << )\n";
-    return std::make_tuple(false, std::numeric_limits<double>::lowest());
+    return std::make_tuple(false, std::numeric_limits<double>::lowest(), false);
   }
 
   // Record these bytes
   rx_state.bytes_received.push_back(std::make_pair(now, num_bytes));
   rx_state.bytes_received_this_epoch += num_bytes;
 
-  return std::make_tuple(true, rx_power);
+  return std::make_tuple(true, rx_power, usingBreadcrumbs);
 }
 
 }

subt-communication/subt_communication_model/tests/unit_test.cpp

@@ -51,10 +51,11 @@ TEST(range_based, co_located)
 
   bool send_packet;
   double rssi;
-  std::tie(send_packet, rssi) = attempt_send(radio,
-                                             tx,  // TX state
-                                             rx,  // RX state
-                                             1000);    // 1Kb packet
+  bool usingBreadcrumbs;
+  std::tie(send_packet, rssi, usingBreadcrumbs) = attempt_send(radio,
+                                                    tx,  // TX state
+                                                    rx,  // RX state
+                                                    1000);    // 1Kb packet
 
   ASSERT_TRUE(send_packet);
 }

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

@@ -65,7 +65,8 @@ struct rf_power
 /// Function signature for computing pathloss.
 typedef std::function<rf_power(const double&, // tx_power
                                radio_state&, // tx_state
-                               radio_state&  //rx_state
+                               radio_state&,  //rx_state
+                               bool& // using breadcumbs?
                                )> pathloss_function;
 
 

subt_ign/include/subt_ign/VisibilityRfModel.hh

@@ -80,12 +80,14 @@ namespace subt
         /// Compute received power function that will be given to
         /// communcation model.
         ///
-        /// @param tx_power Transmit power (dBm)
-        /// @param tx_state Transmitter state
-        /// @param rx_state Receiver state
+        /// @param _txPower Transmit power (dBm)
+        /// @param _txState Transmitter state
+        /// @param _rxState Receiver state
+        /// @param _usingBreadCrumbs True if one or more breadcrumbs were used
         public: rf_power ComputeReceivedPower(const double &_txPower,
                                               radio_state &_txState,
-                                              radio_state &_rxState);
+                                              radio_state &_rxState,
+                                              bool &_usingBreadcrumbs);
 
         /// \brief Whether the visibility model has been successfully
         /// initialized.

subt_ign/src/Common.cc

@@ -106,6 +106,7 @@ bool FullWorldPath(const std::string &_worldName,
   const std::string tunnelPrefix = "tunnel_circuit_";
   const std::string urbanPrefix = "urban_circuit_";
   const std::string cavePrefix = "cave_circuit_";
+  const std::string finalPrefix = "final_event_";
   if (0 == _worldName.compare(0, tunnelPrefix.size(), tunnelPrefix))
   {
     std::string suffix = _worldName.substr(tunnelPrefix.size());
@@ -136,6 +137,12 @@ bool FullWorldPath(const std::string &_worldName,
           "cave_circuit", suffix);
     }
   }
+  else if (_worldName.find(finalPrefix) != std::string::npos)
+  {
+    std::string suffix = _worldName.substr(finalPrefix.size());
+    worldsDirectory = ignition::common::joinPaths(worldsDirectory,
+        "final_event", suffix);
+  }
   else if (_worldName.find("simple") == std::string::npos &&
            _worldName.find("_qual") == std::string::npos &&
            _worldName.find("_stix") == std::string::npos &&

subt_ign/src/CommsBrokerPlugin.cc

@@ -182,7 +182,8 @@ bool CommsBrokerPlugin::Load(const tinyxml2::XMLElement *_elem)
                 this->visibilityModel.get(),
                 std::placeholders::_1,
                 std::placeholders::_2,
-                std::placeholders::_3);
+                std::placeholders::_3,
+                std::placeholders::_4);
   }
 
   // Default comms model type is log_normal_range (will always work)

subt_ign/src/VisibilityRfModel.cc

@@ -66,7 +66,8 @@ bool VisibilityModel::Initialized() const
 /////////////////////////////////////////////
 rf_power VisibilityModel::ComputeReceivedPower(const double &_txPower,
                                                radio_state &_txState,
-                                               radio_state &_rxState)
+                                               radio_state &_rxState,
+                                               bool &_usingBreadcrumbs)
 {
   // Use this->visibilityTable.Cost(_txState, _rxState) to compute
   // pathloss and thus, received power
@@ -87,11 +88,13 @@ rf_power VisibilityModel::ComputeReceivedPower(const double &_txPower,
   if (visibilityCost.route.empty())
   {
     // No breadcrumbs in the route
+    _usingBreadcrumbs = false;
     range = _txState.pose.Pos().Distance(_rxState.pose.Pos());
   }
   else
   {
     // One or more breadcrumbs to cross.
+    _usingBreadcrumbs = true;
     double distSourceToFirstBreadcrumb =
       _txState.pose.Pos().Distance(visibilityCost.posFirstBreadcrumb);
     double distLastBreadcrumbToDestination =
@@ -235,7 +238,8 @@ bool VisibilityModel::VisualizeVisibility(const ignition::msgs::StringMsg &_req,
       // Set and calculate rf power
       tx.pose.Set(from.X(), from.Y(), from.Z(), 0, 0, 0);
       rx.pose.Set(to.X(), to.Y(), to.Z(), 0, 0, 0);
-      rf_power rf_pow = ComputeReceivedPower(txPower, tx, rx);
+      bool usingBreadcrumbs;
+      rf_power rf_pow = ComputeReceivedPower(txPower, tx, rx, usingBreadcrumbs);
       // Based on rx_power, noise value, and modulation, compute the bit error rate (BER)
       double ber = QPSKPowerToBER( dbmToPow(rf_pow.mean), dbmToPow(noise_floor) );
       int num_bytes = 100; // Hardcoded number of bytes