Skip to content

Commit

Permalink
Enable specific entities on buoyancy plugin (#1067)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@chapulina
chapulina authored Sep 30, 2021
1 parent cff085e commit d6fbc26
Show file tree
Hide file tree
Showing 6 changed files with 268 additions and 158 deletions.
212 changes: 96 additions & 116 deletions examples/worlds/graded_buoyancy.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,74 +33,18 @@
<density>1</density>
</density_change>
</graded_buoyancy>
</plugin>

<gui fullscreen="0">

<!-- 3D scene -->
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>

<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-10 -2 5.6 0 0.5 0</camera_pose>
</plugin>

<!-- World control -->
<plugin filename="WorldControl" name="World control">
<ignition-gui>
<title>World control</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">72</property>
<property type="double" key="width">121</property>
<property type="double" key="z">1</property>

<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="left" target="left"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>
<!-- enable by link name -->
<enable>lighter_than_water::ball::body</enable>

<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>
<service>/world/buoyancy/control</service>
<stats_topic>/world/buoyancy/stats</stats_topic>
</plugin>
<!-- enable by nested model name -->
<enable>lighter_than_water::box</enable>

<!-- World statistics -->
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>

<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>

<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
<topic>/world/buoyancy/stats</topic>
</plugin>

</gui>
<!-- enable by top level model name -->
<enable>balloon_lighter_than_air</enable>
<enable>box_neutral_buoyancy</enable>
<enable>box_negative_buoyancy</enable>
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
Expand Down Expand Up @@ -135,13 +79,82 @@
</link>
</model>

<!-- This sphere should bob up and down. -->
<model name='ball_lighter_than_water'>
<model name='lighter_than_water'>
<pose>0 0 0 0 0 0</pose>
<!-- This sphere should bob up and down. -->
<model name="ball">
<link name='body'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>25</mass>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<visual name='body_visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</visual>
<collision name='body_collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
</model>

<!-- This box should float. -->
<model name='box'>
<pose>3 5 0 0.3 0.2 0.1</pose>
<link name='body'>
<inertial>
<mass>200</mass>
<inertia>
<ixx>33.33</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>33.33</iyy>
<iyz>0</iyz>
<izz>33.33</izz>
</inertia>
</inertial>

<visual name='body_visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<collision name='body_collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
</model>
</model>

<!-- This balloon should shoot up -->
<model name='balloon_lighter_than_air'>
<pose>0 -5 -5 0 0 0</pose>
<link name='body'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>25</mass>
<mass>0.1</mass>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
Expand Down Expand Up @@ -169,19 +182,20 @@
</link>
</model>

<!-- This box should float. -->
<model name='box_lighter_than_water'>
<pose>3 5 0 0.3 0.2 0.1</pose>
<!-- This box is neutrally buoyant and therefore should stay still -->
<model name='box_neutral_buoyancy'>
<pose>0 5 -3 0 0 0</pose>
<link name='body'>
<inertial>
<mass>200</mass>
<mass>1000</mass>
<pose>0 0 0.1 0 0 0</pose>
<inertia>
<ixx>33.33</ixx>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>33.33</iyy>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>33.33</izz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

Expand All @@ -202,46 +216,12 @@
</link>
</model>

<!-- This balloon should shoot up -->
<model name='balloon_lighter_than_air'>
<pose>0 -5 -5 0 0 0</pose>
<link name='body'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<visual name='body_visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</visual>
<collision name='body_collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
</model>

<!-- This box is neutrally buoyant and therefore should stay still -->
<model name='box_neutral_buoyancy'>
<pose>0 5 -3 0 0 0</pose>
<!-- This box is negatively buoyant and therefore should sink -->
<model name='box_negative_buoyancy'>
<pose>0 -8 0 0 0 0</pose>
<link name='body'>
<inertial>
<mass>1000</mass>
<mass>1050</mass>
<pose>0 0 0.1 0 0 0</pose>
<inertia>
<ixx>86.28907821859966</ixx>
Expand Down Expand Up @@ -270,9 +250,9 @@
</link>
</model>

<!-- This box is negatively buoyant and therefore should sink -->
<model name='box_negative_buoyancy'>
<pose>0 -8 0 0 0 0</pose>
<!-- Not affected by buoyancy -->
<model name='box_no_buoyancy'>
<pose>4 -6 0 0 0 0</pose>
<link name='body'>
<inertial>
<mass>1050</mass>
Expand Down
2 changes: 1 addition & 1 deletion src/gui/plugins/video_recorder/VideoRecorder.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ namespace gazebo
override;

// Documentation inherited
public: bool eventFilter(QObject *_obj, QEvent *_event);
public: bool eventFilter(QObject *_obj, QEvent *_event) override;

/// \brief Callback when video record start request is received
/// \param[in] _format Video encoding format
Expand Down
64 changes: 62 additions & 2 deletions src/systems/buoyancy/Buoyancy.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <map>
#include <mutex>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>

Expand All @@ -41,6 +42,7 @@
#include "ignition/gazebo/components/Gravity.hh"
#include "ignition/gazebo/components/Inertial.hh"
#include "ignition/gazebo/components/Link.hh"
#include "ignition/gazebo/components/ParentEntity.hh"
#include "ignition/gazebo/components/Pose.hh"
#include "ignition/gazebo/components/Volume.hh"
#include "ignition/gazebo/components/World.hh"
Expand Down Expand Up @@ -121,6 +123,10 @@ class ignition::gazebo::systems::BuoyancyPrivate
/// at _pose.
public: std::pair<math::Vector3d, math::Vector3d> ResolveForces(
const math::Pose3d &_pose);

/// \brief Scoped names of entities that buoyancy should apply to. If empty,
/// all links will receive buoyancy.
public: std::unordered_set<std::string> enabled;
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -293,6 +299,16 @@ void Buoyancy::Configure(const Entity &_entity,
argument = argument->GetNextElement();
}
}

if (_sdf->HasElement("enable"))
{
for (auto enableElem = _sdf->FindElement("enable");
enableElem != nullptr;
enableElem = enableElem->GetNextElement("enable"))
{
this->dataPtr->enabled.insert(enableElem->Get<std::string>());
}
}
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -324,6 +340,11 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
return true;
}

if (!this->IsEnabled(_entity, _ecm))
{
return true;
}

Link link(_entity);

std::vector<Entity> collisions = _ecm.ChildrenByComponents(
Expand Down Expand Up @@ -488,9 +509,16 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
gravity->Data());
break;
default:
ignwarn << "Only <box> and <sphere> collisions are supported by "
<< "the graded buoyancy option." << std::endl;
{
static bool warned{false};
if (!warned)
{
ignwarn << "Only <box> and <sphere> collisions are supported "
<< "by the graded buoyancy option." << std::endl;
warned = true;
}
break;
}
}
}
}
Expand All @@ -503,6 +531,38 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
});
}

//////////////////////////////////////////////////
bool Buoyancy::IsEnabled(Entity _entity,
const EntityComponentManager &_ecm) const
{
// If there's nothing enabled, all entities are enabled
if (this->dataPtr->enabled.empty())
return true;

auto entity = _entity;
while (entity != kNullEntity)
{
// Fully scoped name
auto name = scopedName(entity, _ecm, "::", false);

// Remove world name
name = removeParentScope(name, "::");

if (this->dataPtr->enabled.find(name) != this->dataPtr->enabled.end())
return true;

// Check parent
auto parentComp = _ecm.Component<components::ParentEntity>(entity);

if (nullptr == parentComp)
return false;

entity = parentComp->Data();
}

return false;
}

IGNITION_ADD_PLUGIN(Buoyancy,
ignition::gazebo::System,
Buoyancy::ISystemConfigure,
Expand Down
Loading

0 comments on commit d6fbc26

Please sign in to comment.