Skip to content

Commit

Permalink
Enable breadcrumbs for X1 and X2.
Browse files Browse the repository at this point in the history
  • Loading branch information
@caguero
caguero committed Mar 20, 2020
1 parent 5647333 commit d228a87
Show file tree
Hide file tree
Showing 5 changed files with 155 additions and 0 deletions.
9 changes: 9 additions & 0 deletions subt_ign/include/subt_ign/CommsBrokerPlugin.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;
};
}

Expand Down
22 changes: 22 additions & 0 deletions subt_ign/include/subt_ign/VisibilityRfModel.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <subt_rf_interface/subt_rf_model.h>

#include <map>
#include <set>
#include <string>

#include <ignition/msgs.hh>
Expand Down Expand Up @@ -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);
Expand Down
88 changes: 88 additions & 0 deletions subt_ign/launch/cave_circuit.ign
View file
Original file line number Diff line number Diff line change
Expand Up @@ -474,6 +474,50 @@
" <update_rate>10</update_rate>"\
" <type>gas</type>"\
" </plugin>\n"\
" <plugin filename=\"libignition-gazebo-breadcrumbs-system.so\"\n"\
" name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
" <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
" <max_deployments>3</max_deployments>"\
" <breadcrumb>\n"\
" <sdf version=\"1.6\">\n"\
" <model name=\"#{_name}__breadcrumb__\">\n"\
" <pose>-1.2 0 0 0 0 0</pose>\n"\
" <link name='body'>\n"\
" <inertial>\n"\
" <mass>0.5</mass>\n"\
" <inertia>\n"\
" <ixx>0.0008</ixx>\n"\
" <ixy>0</ixy>\n"\
" <ixz>0</ixz>\n"\
" <iyy>0.0008</iyy>\n"\
" <iyz>0</iyz>\n"\
" <izz>0.0008</izz>\n"\
" </inertia>\n"\
" </inertial>\n"\
" <visual name='visual'>\n"\
" <geometry>\n"\
" <box>\n"\
" <size>0.1 0.1 0.1</size>\n"\
" </box>\n"\
" </geometry>\n"\
" <material>\n"\
" <ambient>1.0 0.0 0.0 1</ambient>\n"\
" <diffuse>1.0 0.0 0.0 1</diffuse>\n"\
" <specular>0.5 0.5 0.5 1</specular>\n"\
" </material>\n"\
" </visual>\n"\
" <collision name='collision'>\n"\
" <geometry>\n"\
" <box>\n"\
" <size>0.1 0.1 0.1</size>\n"\
" </box>\n"\
" </geometry>\n"\
" </collision>\n"\
" </link>\n"\
" </model>\n"\
" </sdf>\n"\
" </breadcrumb>\n"\
" </plugin>\n"\
" </include>\n"\
" </sdf>\n"\
"</plugin>\n"\
Expand Down Expand Up @@ -554,6 +598,50 @@
" <update_rate>10</update_rate>"\
" <type>gas</type>"\
" </plugin>\n"\
" <plugin filename=\"libignition-gazebo-breadcrumbs-system.so\"\n"\
" name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
" <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
" <max_deployments>3</max_deployments>"\
" <breadcrumb>\n"\
" <sdf version=\"1.6\">\n"\
" <model name=\"#{_name}__breadcrumb__\">\n"\
" <pose>-1.2 0 0 0 0 0</pose>\n"\
" <link name='body'>\n"\
" <inertial>\n"\
" <mass>0.5</mass>\n"\
" <inertia>\n"\
" <ixx>0.0008</ixx>\n"\
" <ixy>0</ixy>\n"\
" <ixz>0</ixz>\n"\
" <iyy>0.0008</iyy>\n"\
" <iyz>0</iyz>\n"\
" <izz>0.0008</izz>\n"\
" </inertia>\n"\
" </inertial>\n"\
" <visual name='visual'>\n"\
" <geometry>\n"\
" <box>\n"\
" <size>0.1 0.1 0.1</size>\n"\
" </box>\n"\
" </geometry>\n"\
" <material>\n"\
" <ambient>0.0 1.0 0.0 1</ambient>\n"\
" <diffuse>0.0 1.0 0.0 1</diffuse>\n"\
" <specular>0.5 0.5 0.5 1</specular>\n"\
" </material>\n"\
" </visual>\n"\
" <collision name='collision'>\n"\
" <geometry>\n"\
" <box>\n"\
" <size>0.1 0.1 0.1</size>\n"\
" </box>\n"\
" </geometry>\n"\
" </collision>\n"\
" </link>\n"\
" </model>\n"\
" </sdf>\n"\
" </breadcrumb>\n"\
" </plugin>\n"\
" </include>\n"\
" </sdf>\n"\
"</plugin>\n"\
Expand Down
29 changes: 29 additions & 0 deletions subt_ign/src/CommsBrokerPlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<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;
}
}
7 changes: 7 additions & 0 deletions subt_ign/src/VisibilityRfModel.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit d228a87

Please sign in to comment.