diff --git a/subt_ign/include/subt_ign/CommsBrokerPlugin.hh b/subt_ign/include/subt_ign/CommsBrokerPlugin.hh index 8d68a692..8aa29ec8 100644 --- a/subt_ign/include/subt_ign/CommsBrokerPlugin.hh +++ b/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 breadcrumbs; }; } diff --git a/subt_ign/include/subt_ign/VisibilityRfModel.hh b/subt_ign/include/subt_ign/VisibilityRfModel.hh index 7350e345..6424c0aa 100644 --- a/subt_ign/include/subt_ign/VisibilityRfModel.hh +++ b/subt_ign/include/subt_ign/VisibilityRfModel.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -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 &_relayPoses); + /// Function to visualize visibility cost in Gazebo. private: bool VisualizeVisibility(const ignition::msgs::StringMsg &_req, ignition::msgs::Boolean &_rep); diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign index c807505d..85ae96d5 100644 --- a/subt_ign/launch/cave_circuit.ign +++ b/subt_ign/launch/cave_circuit.ign @@ -474,6 +474,50 @@ " 10"\ " gas"\ " \n"\ + " \n"\ + " /model/#{_name}/breadcrumb/deploy\n"\ + " 3"\ + " \n"\ + " \n"\ + " \n"\ + " -1.2 0 0 0 0 0\n"\ + " \n"\ + " \n"\ + " 0.5\n"\ + " \n"\ + " 0.0008\n"\ + " 0\n"\ + " 0\n"\ + " 0.0008\n"\ + " 0\n"\ + " 0.0008\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " 0.1 0.1 0.1\n"\ + " \n"\ + " \n"\ + " \n"\ + " 1.0 0.0 0.0 1\n"\ + " 1.0 0.0 0.0 1\n"\ + " 0.5 0.5 0.5 1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " 0.1 0.1 0.1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ " \n"\ " \n"\ "\n"\ @@ -554,6 +598,50 @@ " 10"\ " gas"\ " \n"\ + " \n"\ + " /model/#{_name}/breadcrumb/deploy\n"\ + " 3"\ + " \n"\ + " \n"\ + " \n"\ + " -1.2 0 0 0 0 0\n"\ + " \n"\ + " \n"\ + " 0.5\n"\ + " \n"\ + " 0.0008\n"\ + " 0\n"\ + " 0\n"\ + " 0.0008\n"\ + " 0\n"\ + " 0.0008\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " 0.1 0.1 0.1\n"\ + " \n"\ + " \n"\ + " \n"\ + " 0.0 1.0 0.0 1\n"\ + " 0.0 1.0 0.0 1\n"\ + " 0.5 0.5 0.5 1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " 0.1 0.1 0.1\n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ + " \n"\ " \n"\ " \n"\ "\n"\ diff --git a/subt_ign/src/CommsBrokerPlugin.cc b/subt_ign/src/CommsBrokerPlugin.cc index 207655b3..9c458714 100644 --- a/subt_ign/src/CommsBrokerPlugin.cc +++ b/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__") && + this->breadcrumbs.find(name) == this->breadcrumbs.end()) + { + this->breadcrumbs[name] = pose; + newBreadcrumbFound = true; + } + } + + // Update the comms. + if (newBreadcrumbFound) + { + std::set 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; + } +} diff --git a/subt_ign/src/VisibilityRfModel.cc b/subt_ign/src/VisibilityRfModel.cc index f07586e7..0f53ba8c 100644 --- a/subt_ign/src/VisibilityRfModel.cc +++ b/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 &_relayPoses) +{ + this->visibilityTable.PopulateVisibilityInfo(_relayPoses); +} + ///////////////////////////////////////////// bool VisibilityModel::VisualizeVisibility(const ignition::msgs::StringMsg &_req, ignition::msgs::Boolean &_rep)