Merged in breadcrumb_comms_part2 (pull request #422)

Communication support for breadcrumbs - part 2

Approved-by: Nate Koenig <natekoenig@gmail.com>

Author: nkoenig

subt_ign/include/subt_ign/CommsBrokerPlugin.hh

@@ -73,8 +73,13 @@ namespace subt
     public: virtual bool Load(const tinyxml2::XMLElement *_elem) override final;
 
     /// \brief Callback for World Update events.
+    /// \param[in] _msg Vector of entity positions.
     private: void OnPose(const ignition::msgs::Pose_V &_msg);
 
+    /// \brief Update the visibility table if new breadcrumbs are found.
+    /// Note that this function doesn't consider breadcrumb removals.
+    private: void UpdateIfNewBreadcrumbs();
+
     /// \brief Broker instance.
     private: subt::communication_broker::Broker broker;
 
@@ -90,6 +95,10 @@ namespace subt
       visibilityModel;
 
     private: ignition::transport::Node node;
+
+    /// \brief Names and poses of the breadcrumbs.
+    /// The key is the breadcrumb name and the value is its pose.
+    private: std::map<std::string, ignition::math::Pose3d> breadcrumbs;
   };
 }
 

subt_ign/include/subt_ign/VisibilityRfModel.hh

@@ -21,6 +21,7 @@
 #include <subt_rf_interface/subt_rf_model.h>
 
 #include <map>
+#include <set>
 #include <string>
 
 #include <ignition/msgs.hh>
@@ -91,6 +92,27 @@ namespace subt
         /// \return True if initialized or false otherwise.
         public: bool Initialized() 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.
+        /// The cost of the best route is computed as follows:
+        ///   * The direct route without taking into account breadcrumbs is
+        ///     computed.
+        ///   * The best indirect route (using one or more relays) is computed.
+        ///   * The cost of a route that has multiple hops is the cost of the
+        ///     hop with bigger cost.
+        ///   * The total cost is the minimum cost between the direct route and
+        ///     the best indirect route.
+        ///  A few examples using A--(1)--B--(2)--BC--(2)--D--2--E
+        ///  Note that BC is a breadcrumb.
+        ///  Cost(A, A):  0
+        ///  Cost(A, B):  1
+        ///  Cost(A, BC): 3
+        ///  Cost(A, D):  3
+        ///  Cost(A, E):  4
+        public: void PopulateVisibilityInfo(
+                         const std::set<ignition::math::Vector3d> &_relayPoses);
+
         /// Function to visualize visibility cost in Gazebo.
         private: bool VisualizeVisibility(const ignition::msgs::StringMsg &_req,
                                           ignition::msgs::Boolean &_rep);

subt_ign/src/CommsBrokerPlugin.cc

@@ -258,6 +258,9 @@ void CommsBrokerPlugin::OnPose(const ignition::msgs::Pose_V &_msg)
   for (int i = 0; i < _msg.pose_size(); ++i)
     this->poses[_msg.pose(i).name()] = ignition::msgs::Convert(_msg.pose(i));
 
+  // Update the visibility graph if needed.
+  this->UpdateIfNewBreadcrumbs();
+
   // double dt = (this->simTime - this->lastROSParameterCheckTime).Double();
 
   // Todo: Remove this line and enable the block below when ROS parameter
@@ -299,3 +302,29 @@ void CommsBrokerPlugin::OnPose(const ignition::msgs::Pose_V &_msg)
   // the message according to the communication model.
   this->broker.DispatchMessages();
 }
+
+/////////////////////////////////////////////////
+void CommsBrokerPlugin::UpdateIfNewBreadcrumbs()
+{
+  bool newBreadcrumbFound = false;
+  for (const auto& [name, pose] : this->poses)
+  {
+    // New breadcrumb found.
+    if (name.find("__breadcrumb__") != std::string::npos &&
+        this->breadcrumbs.find(name) == this->breadcrumbs.end())
+    {
+      this->breadcrumbs[name] = pose;
+      newBreadcrumbFound = true;
+    }
+  }
+
+  // Update the comms.
+  if (newBreadcrumbFound)
+  {
+    std::set<ignition::math::Vector3d> breadcrumbPoses;
+    for (const auto& [name, pose] : this->breadcrumbs)
+      breadcrumbPoses.insert(pose.Pos());
+    this->visibilityModel->PopulateVisibilityInfo(breadcrumbPoses);
+    ignmsg << "New breadcrumb detected, visibility graph updated" << std::endl;
+  }
+}

subt_ign/src/VisibilityRfModel.cc

@@ -83,6 +83,13 @@ rf_power VisibilityModel::ComputeReceivedPower(const double &_txPower,
   return std::move(rx);
 }
 
+/////////////////////////////////////////////
+void VisibilityModel::PopulateVisibilityInfo(
+  const std::set<ignition::math::Vector3d> &_relayPoses)
+{
+  this->visibilityTable.PopulateVisibilityInfo(_relayPoses);
+}
+
 /////////////////////////////////////////////
 bool VisibilityModel::VisualizeVisibility(const ignition::msgs::StringMsg &_req,
                                           ignition::msgs::Boolean &_rep)

subt_ign/src/VisibilityTable.cc

@@ -270,7 +270,16 @@ void VisibilityTable::PopulateVisibilityInfo(
   // Convert poses to vertices.
   std::set<ignition::math::graph::VertexId> relays;
   for (const auto pose : _relayPoses)
-    relays.insert(this->Index(pose));
+  {
+    int32_t x = std::round(pose.X());
+    int32_t y = std::round(pose.Y());
+    int32_t z = std::round(pose.Z());
+    auto vertexId = std::make_tuple(x, y, z);
+
+    auto it = this->vertices.find(vertexId);
+    if (it != this->vertices.end())
+      relays.insert(it->second);
+  }
 
   // Compute the cost of all routes without considering relays.
   this->PopulateVisibilityInfo();

subt_ros/launch/vehicle_topics.launch

@@ -166,6 +166,13 @@
         args="/model/$(arg name)/odometry@nav_msgs/Odometry[ignition.msgs.Odometry">
         <remap from="/model/$(arg name)/odometry" to="odom"/>
       </node>
+      <node
+        pkg="ros_ign_bridge"
+        type="parameter_bridge"
+        name="ros_ign_bridge_breadcrumbs"
+        args="/model/$(arg name)/breadcrumb/deploy@std_msgs/Empty]ignition.msgs.Empty">
+        <remap from="/model/$(arg name)/breadcrumb/deploy" to="breadcrumb/deploy"/>
+      </node>
     </group>
 
     <node