Skip to content

Commit

Permalink
merged from ign-gazebo3
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@chapulina
chapulina committed Jul 29, 2021
2 parents d8c6047 + eae6b9c commit aabd086
Show file tree
Hide file tree
Showing 12 changed files with 1,412 additions and 4 deletions.
152 changes: 152 additions & 0 deletions examples/worlds/plot_3d.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,152 @@
<?xml version="1.0" ?>
<!--
Demo the Plot3D plugin.
Start this world and see how the pendulum's link's paths are plotted in the 3D scene as it moves.
The plugin is instantiated twice, once for each link of the pendulum.
Try out different parameters in each instance of the plugin to see how the plot changes.
-->
<sdf version="1.6">
<world name="shapes">

<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>5 -1.5 3 0 0.37 2.8</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>

<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>
</plugin>

<!-- 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>
</plugin>

<!-- Entity tree -->
<plugin filename="EntityTree" name="Entity tree">
</plugin>

<!-- Plot 3D: upper link -->
<plugin filename="Plot3D" name="Plot 3D">
<ignition-gui>
<title>Upper link plot</title>
</ignition-gui>
<entity_name>Double_pendulum::upper_link</entity_name>
<color>0 0 1</color>
<offset>0 0 1</offset>
<maximum_points>20</maximum_points>
<minimum_distance>0.1</minimum_distance>
</plugin>

<!-- Plot 3D: lower link -->
<plugin filename="Plot3D" name="Plot 3D">
<ignition-gui>
<title>Lower link plot</title>
</ignition-gui>
<entity_name>Double_pendulum::lower_link</entity_name>
<color>0 1 0</color>
<offset>0 0 1</offset>
<maximum_points>40</maximum_points>
<minimum_distance>0.3</minimum_distance>
</plugin>
</gui>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<include>
<name>Double_pendulum</name>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base</uri>
</include>

</world>
</sdf>
29 changes: 29 additions & 0 deletions include/ignition/gazebo/Util.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#define IGNITION_GAZEBO_UTIL_HH_

#include <string>
#include <unordered_set>
#include <vector>

#include <ignition/math/Pose3.hh>
Expand Down Expand Up @@ -51,6 +52,34 @@ namespace ignition
const EntityComponentManager &_ecm, const std::string &_delim = "/",
bool _includePrefix = true);

/// \brief Helper function to get an entity given its scoped name.
/// The scope may start at any level by default. For example, in this
/// hierarchy:
///
/// world_name
/// model_name
/// link_name
///
/// All these names will return the link entity:
///
/// * world_name::model_name::link_name
/// * model_name::link_name
/// * link_name
///
/// \param[in] _scopedName Entity's scoped name.
/// \param[in] _ecm Immutable reference to ECM.
/// \param[in] _relativeTo Entity that the scoped name is relative to. The
/// scoped name does not include the name of this entity. If not provided,
/// the scoped name could be relative to any entity.
/// \param[in] _delim Delimiter between names, defaults to "::", it can't
/// be empty.
/// \return All entities that match the scoped name and relative to
/// requirements, or an empty set otherwise.
std::unordered_set<Entity> IGNITION_GAZEBO_VISIBLE entitiesFromScopedName(
const std::string &_scopedName, const EntityComponentManager &_ecm,
Entity _relativeTo = kNullEntity,
const std::string &_delim = "::");

/// \brief Generally, each entity will be of some specific high-level type,
/// such as World, Sensor, Collision, etc, and one type only.
/// The entity type is usually marked by having some component that
Expand Down
68 changes: 67 additions & 1 deletion src/Util.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
math::Pose3d worldPose(const Entity &_entity,
const EntityComponentManager &_ecm)
{
auto poseComp = _ecm.Component<components::Pose>(_entity);
if (nullptr == poseComp)
{
ignwarn << "Trying to get world pose from entity [" << _entity
<< "], which doesn't have a pose component" << std::endl;
return math::Pose3d();
}

// work out pose in world frame
math::Pose3d pose = _ecm.Component<components::Pose>(_entity)->Data();
math::Pose3d pose = poseComp->Data();
auto p = _ecm.Component<components::ParentEntity>(_entity);
while (p)
{
Expand Down Expand Up @@ -125,6 +133,64 @@ std::string scopedName(const Entity &_entity,
return result;
}

//////////////////////////////////////////////////
std::unordered_set<Entity> entitiesFromScopedName(
const std::string &_scopedName, const EntityComponentManager &_ecm,
Entity _relativeTo, const std::string &_delim)
{
if (_delim.empty())
{
ignwarn << "Can't process scoped name [" << _scopedName
<< "] with empty delimiter." << std::endl;
return {};
}

// Split names
std::vector<std::string> names;
size_t pos1 = 0;
size_t pos2 = _scopedName.find(_delim);
while (pos2 != std::string::npos)
{
names.push_back(_scopedName.substr(pos1, pos2 - pos1));
pos1 = pos2 + _delim.length();
pos2 = _scopedName.find(_delim, pos1);
}
names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1));

// Holds current entities that match and is updated for each name
std::vector<Entity> resVector;

// If there's an entity we're relative to, treat it as the first level result
if (_relativeTo != kNullEntity)
{
resVector = {_relativeTo};
}

for (const auto &name : names)
{
std::vector<Entity> current;
if (resVector.empty())
{
current = _ecm.EntitiesByComponents(components::Name(name));
}
else
{
for (auto res : resVector)
{
auto matches = _ecm.EntitiesByComponents(components::Name(name),
components::ParentEntity(res));
std::copy(std::begin(matches), std::end(matches),
std::back_inserter(current));
}
}
if (current.empty())
return {};
resVector = current;
}

return std::unordered_set<Entity>(resVector.begin(), resVector.end());
}

//////////////////////////////////////////////////
ComponentTypeId entityTypeId(const Entity &_entity,
const EntityComponentManager &_ecm)
Expand Down
82 changes: 82 additions & 0 deletions src/Util_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,88 @@ TEST_F(UtilTest, ScopedName)
EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm));
}

/////////////////////////////////////////////////
TEST_F(UtilTest, EntitiesFromScopedName)
{
EntityComponentManager ecm;

// banana 1
// - orange 2
// - plum 3
// - grape 4
// - pear 5
// - plum 6
// - grape 7
// - pear 8
// - plum 9
// - pear 10
// - grape 11
// - pear 12
// - orange 13
// - orange 14
// - pear 15

auto createEntity = [&ecm](const std::string &_name, Entity _parent) -> Entity
{
auto res = ecm.CreateEntity();
ecm.CreateComponent(res, components::Name(_name));
ecm.CreateComponent(res, components::ParentEntity(_parent));
return res;
};

auto banana1 = ecm.CreateEntity();
ecm.CreateComponent(banana1, components::Name("banana"));

auto orange2 = createEntity("orange", banana1);
auto plum3 = createEntity("plum", orange2);
auto grape4 = createEntity("grape", plum3);
auto pear5 = createEntity("pear", grape4);
auto plum6 = createEntity("plum", pear5);
auto grape7 = createEntity("grape", banana1);
auto pear8 = createEntity("pear", grape7);
auto plum9 = createEntity("plum", pear8);
auto pear10 = createEntity("pear", plum9);
auto grape11 = createEntity("grape", banana1);
auto pear12 = createEntity("pear", grape11);
auto orange13 = createEntity("orange", pear12);
auto orange14 = createEntity("orange", orange13);
auto pear15 = createEntity("pear", grape11);

auto checkEntities = [&ecm](const std::string &_scopedName,
Entity _relativeTo, const std::unordered_set<Entity> &_result,
const std::string &_delim)
{
auto res = gazebo::entitiesFromScopedName(_scopedName, ecm, _relativeTo,
_delim);
EXPECT_EQ(_result.size(), res.size()) << _scopedName;

for (auto it : _result)
{
EXPECT_NE(res.find(it), res.end()) << it << " " << _scopedName;
}
};

checkEntities("watermelon", kNullEntity, {}, "::");
checkEntities("banana", kNullEntity, {banana1}, "::");
checkEntities("orange", kNullEntity, {orange2, orange13, orange14}, ":");
checkEntities("banana::orange", kNullEntity, {orange2}, "::");
checkEntities("banana::grape", kNullEntity, {grape7, grape11}, "::");
checkEntities("grape/pear", kNullEntity, {pear5, pear8, pear12, pear15}, "/");
checkEntities("grape...pear...plum", kNullEntity, {plum6, plum9}, "...");
checkEntities(
"banana::orange::plum::grape::pear::plum", kNullEntity, {plum6}, "::");
checkEntities(
"banana::orange::kiwi::grape::pear::plum", kNullEntity, {}, "::");
checkEntities("orange+orange", kNullEntity, {orange14}, "+");
checkEntities("orange", banana1, {orange2}, "::");
checkEntities("grape", banana1, {grape7, grape11}, "::");
checkEntities("orange", orange2, {}, "::");
checkEntities("orange", orange13, {orange14}, "::");
checkEntities("grape::pear::plum", plum3, {plum6}, "::");
checkEntities("pear", grape11, {pear12, pear15}, "==");
checkEntities("plum=pear", pear8, {pear10}, "=");
}

/////////////////////////////////////////////////
TEST_F(UtilTest, EntityTypeId)
{
Expand Down
1 change: 1 addition & 0 deletions src/gui/plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ add_subdirectory(entity_tree)
add_subdirectory(grid_config)
add_subdirectory(joint_position_controller)
add_subdirectory(playback_scrubber)
add_subdirectory(plot_3d)
add_subdirectory(resource_spawner)
add_subdirectory(scene3d)
add_subdirectory(shapes)
Expand Down
8 changes: 8 additions & 0 deletions src/gui/plugins/plot_3d/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
gz_add_gui_plugin(Plot3D
SOURCES
Plot3D.cc
QT_HEADERS
Plot3D.hh
TEST_SOURCES
Plot3D_TEST.cc
)
Loading

0 comments on commit aabd086

Please sign in to comment.