Skip to content

Commit 2ba1658

Browse files
chapulinachapulina
authored
Merge 362570b into becec7e 
2 parents becec7e + 362570b commit 2ba1658

File tree

12 files changed

+1412
-4
lines changed

12 files changed

+1412
-4
lines changed

‎examples/worlds/plot_3d.sdf

+152
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
<?xml version="1.0" ?>
2+
<!--
3+
Demo the Plot3D plugin.
4+
5+
Start this world and see how the pendulum's link's paths are plotted in the 3D scene as it moves.
6+
7+
The plugin is instantiated twice, once for each link of the pendulum.
8+
9+
Try out different parameters in each instance of the plugin to see how the plot changes.
10+
-->
11+
<sdf version="1.6">
12+
<world name="shapes">
13+
14+
<gui fullscreen="0">
15+
16+
<!-- 3D scene -->
17+
<plugin filename="GzScene3D" name="3D View">
18+
<ignition-gui>
19+
<title>3D View</title>
20+
<property type="bool" key="showTitleBar">false</property>
21+
<property type="string" key="state">docked</property>
22+
</ignition-gui>
23+
24+
<engine>ogre2</engine>
25+
<scene>scene</scene>
26+
<ambient_light>0.4 0.4 0.4</ambient_light>
27+
<background_color>0.8 0.8 0.8</background_color>
28+
<camera_pose>5 -1.5 3 0 0.37 2.8</camera_pose>
29+
</plugin>
30+
31+
<!-- World control -->
32+
<plugin filename="WorldControl" name="World control">
33+
<ignition-gui>
34+
<title>World control</title>
35+
<property type="bool" key="showTitleBar">false</property>
36+
<property type="bool" key="resizable">false</property>
37+
<property type="double" key="height">72</property>
38+
<property type="double" key="width">121</property>
39+
<property type="double" key="z">1</property>
40+
41+
<property type="string" key="state">floating</property>
42+
<anchors target="3D View">
43+
<line own="left" target="left"/>
44+
<line own="bottom" target="bottom"/>
45+
</anchors>
46+
</ignition-gui>
47+
48+
<play_pause>true</play_pause>
49+
<step>true</step>
50+
<start_paused>true</start_paused>
51+
</plugin>
52+
53+
<!-- World statistics -->
54+
<plugin filename="WorldStats" name="World stats">
55+
<ignition-gui>
56+
<title>World stats</title>
57+
<property type="bool" key="showTitleBar">false</property>
58+
<property type="bool" key="resizable">false</property>
59+
<property type="double" key="height">110</property>
60+
<property type="double" key="width">290</property>
61+
<property type="double" key="z">1</property>
62+
63+
<property type="string" key="state">floating</property>
64+
<anchors target="3D View">
65+
<line own="right" target="right"/>
66+
<line own="bottom" target="bottom"/>
67+
</anchors>
68+
</ignition-gui>
69+
70+
<sim_time>true</sim_time>
71+
<real_time>true</real_time>
72+
<real_time_factor>true</real_time_factor>
73+
<iterations>true</iterations>
74+
</plugin>
75+
76+
<!-- Entity tree -->
77+
<plugin filename="EntityTree" name="Entity tree">
78+
</plugin>
79+
80+
<!-- Plot 3D: upper link -->
81+
<plugin filename="Plot3D" name="Plot 3D">
82+
<ignition-gui>
83+
<title>Upper link plot</title>
84+
</ignition-gui>
85+
<entity_name>Double_pendulum::upper_link</entity_name>
86+
<color>0 0 1</color>
87+
<offset>0 0 1</offset>
88+
<maximum_points>20</maximum_points>
89+
<minimum_distance>0.1</minimum_distance>
90+
</plugin>
91+
92+
<!-- Plot 3D: lower link -->
93+
<plugin filename="Plot3D" name="Plot 3D">
94+
<ignition-gui>
95+
<title>Lower link plot</title>
96+
</ignition-gui>
97+
<entity_name>Double_pendulum::lower_link</entity_name>
98+
<color>0 1 0</color>
99+
<offset>0 0 1</offset>
100+
<maximum_points>40</maximum_points>
101+
<minimum_distance>0.3</minimum_distance>
102+
</plugin>
103+
</gui>
104+
105+
<light type="directional" name="sun">
106+
<cast_shadows>true</cast_shadows>
107+
<pose>0 0 10 0 0 0</pose>
108+
<diffuse>0.8 0.8 0.8 1</diffuse>
109+
<specular>0.2 0.2 0.2 1</specular>
110+
<attenuation>
111+
<range>1000</range>
112+
<constant>0.9</constant>
113+
<linear>0.01</linear>
114+
<quadratic>0.001</quadratic>
115+
</attenuation>
116+
<direction>-0.5 0.1 -0.9</direction>
117+
</light>
118+
119+
<model name="ground_plane">
120+
<static>true</static>
121+
<link name="link">
122+
<collision name="collision">
123+
<geometry>
124+
<plane>
125+
<normal>0 0 1</normal>
126+
<size>100 100</size>
127+
</plane>
128+
</geometry>
129+
</collision>
130+
<visual name="visual">
131+
<geometry>
132+
<plane>
133+
<normal>0 0 1</normal>
134+
<size>100 100</size>
135+
</plane>
136+
</geometry>
137+
<material>
138+
<ambient>0.8 0.8 0.8 1</ambient>
139+
<diffuse>0.8 0.8 0.8 1</diffuse>
140+
<specular>0.8 0.8 0.8 1</specular>
141+
</material>
142+
</visual>
143+
</link>
144+
</model>
145+
146+
<include>
147+
<name>Double_pendulum</name>
148+
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base</uri>
149+
</include>
150+
151+
</world>
152+
</sdf>

‎include/ignition/gazebo/Util.hh

+29
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#define IGNITION_GAZEBO_UTIL_HH_
1919

2020
#include <string>
21+
#include <unordered_set>
2122
#include <vector>
2223

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

55+
/// \brief Helper function to get an entity given its scoped name.
56+
/// The scope may start at any level by default. For example, in this
57+
/// hierarchy:
58+
///
59+
/// world_name
60+
/// model_name
61+
/// link_name
62+
///
63+
/// All these names will return the link entity:
64+
///
65+
/// * world_name::model_name::link_name
66+
/// * model_name::link_name
67+
/// * link_name
68+
///
69+
/// \param[in] _scopedName Entity's scoped name.
70+
/// \param[in] _ecm Immutable reference to ECM.
71+
/// \param[in] _relativeTo Entity that the scoped name is relative to. The
72+
/// scoped name does not include the name of this entity. If not provided,
73+
/// the scoped name could be relative to any entity.
74+
/// \param[in] _delim Delimiter between names, defaults to "::", it can't
75+
/// be empty.
76+
/// \return All entities that match the scoped name and relative to
77+
/// requirements, or an empty set otherwise.
78+
std::unordered_set<Entity> IGNITION_GAZEBO_VISIBLE entitiesFromScopedName(
79+
const std::string &_scopedName, const EntityComponentManager &_ecm,
80+
Entity _relativeTo = kNullEntity,
81+
const std::string &_delim = "::");
82+
5483
/// \brief Generally, each entity will be of some specific high-level type,
5584
/// such as World, Sensor, Collision, etc, and one type only.
5685
/// The entity type is usually marked by having some component that

‎src/Util.cc

+67-1
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
5959
math::Pose3d worldPose(const Entity &_entity,
6060
const EntityComponentManager &_ecm)
6161
{
62+
auto poseComp = _ecm.Component<components::Pose>(_entity);
63+
if (nullptr == poseComp)
64+
{
65+
ignwarn << "Trying to get world pose from entity [" << _entity
66+
<< "], which doesn't have a pose component" << std::endl;
67+
return math::Pose3d();
68+
}
69+
6270
// work out pose in world frame
63-
math::Pose3d pose = _ecm.Component<components::Pose>(_entity)->Data();
71+
math::Pose3d pose = poseComp->Data();
6472
auto p = _ecm.Component<components::ParentEntity>(_entity);
6573
while (p)
6674
{
@@ -125,6 +133,64 @@ std::string scopedName(const Entity &_entity,
125133
return result;
126134
}
127135

136+
//////////////////////////////////////////////////
137+
std::unordered_set<Entity> entitiesFromScopedName(
138+
const std::string &_scopedName, const EntityComponentManager &_ecm,
139+
Entity _relativeTo, const std::string &_delim)
140+
{
141+
if (_delim.empty())
142+
{
143+
ignwarn << "Can't process scoped name [" << _scopedName
144+
<< "] with empty delimiter." << std::endl;
145+
return {};
146+
}
147+
148+
// Split names
149+
std::vector<std::string> names;
150+
size_t pos1 = 0;
151+
size_t pos2 = _scopedName.find(_delim);
152+
while (pos2 != std::string::npos)
153+
{
154+
names.push_back(_scopedName.substr(pos1, pos2 - pos1));
155+
pos1 = pos2 + _delim.length();
156+
pos2 = _scopedName.find(_delim, pos1);
157+
}
158+
names.push_back(_scopedName.substr(pos1, _scopedName.size()-pos1));
159+
160+
// Holds current entities that match and is updated for each name
161+
std::vector<Entity> resVector;
162+
163+
// If there's an entity we're relative to, treat it as the first level result
164+
if (_relativeTo != kNullEntity)
165+
{
166+
resVector = {_relativeTo};
167+
}
168+
169+
for (const auto &name : names)
170+
{
171+
std::vector<Entity> current;
172+
if (resVector.empty())
173+
{
174+
current = _ecm.EntitiesByComponents(components::Name(name));
175+
}
176+
else
177+
{
178+
for (auto res : resVector)
179+
{
180+
auto matches = _ecm.EntitiesByComponents(components::Name(name),
181+
components::ParentEntity(res));
182+
std::copy(std::begin(matches), std::end(matches),
183+
std::back_inserter(current));
184+
}
185+
}
186+
if (current.empty())
187+
return {};
188+
resVector = current;
189+
}
190+
191+
return std::unordered_set<Entity>(resVector.begin(), resVector.end());
192+
}
193+
128194
//////////////////////////////////////////////////
129195
ComponentTypeId entityTypeId(const Entity &_entity,
130196
const EntityComponentManager &_ecm)

‎src/Util_TEST.cc

+82
Original file line numberDiff line numberDiff line change
@@ -218,6 +218,88 @@ TEST(UtilTest, ScopedName)
218218
EXPECT_EQ(kNullEntity, gazebo::worldEntity(kNullEntity, ecm));
219219
}
220220

221+
/////////////////////////////////////////////////
222+
TEST(UtilTest, EntitiesFromScopedName)
223+
{
224+
EntityComponentManager ecm;
225+
226+
// banana 1
227+
// - orange 2
228+
// - plum 3
229+
// - grape 4
230+
// - pear 5
231+
// - plum 6
232+
// - grape 7
233+
// - pear 8
234+
// - plum 9
235+
// - pear 10
236+
// - grape 11
237+
// - pear 12
238+
// - orange 13
239+
// - orange 14
240+
// - pear 15
241+
242+
auto createEntity = [&ecm](const std::string &_name, Entity _parent) -> Entity
243+
{
244+
auto res = ecm.CreateEntity();
245+
ecm.CreateComponent(res, components::Name(_name));
246+
ecm.CreateComponent(res, components::ParentEntity(_parent));
247+
return res;
248+
};
249+
250+
auto banana1 = ecm.CreateEntity();
251+
ecm.CreateComponent(banana1, components::Name("banana"));
252+
253+
auto orange2 = createEntity("orange", banana1);
254+
auto plum3 = createEntity("plum", orange2);
255+
auto grape4 = createEntity("grape", plum3);
256+
auto pear5 = createEntity("pear", grape4);
257+
auto plum6 = createEntity("plum", pear5);
258+
auto grape7 = createEntity("grape", banana1);
259+
auto pear8 = createEntity("pear", grape7);
260+
auto plum9 = createEntity("plum", pear8);
261+
auto pear10 = createEntity("pear", plum9);
262+
auto grape11 = createEntity("grape", banana1);
263+
auto pear12 = createEntity("pear", grape11);
264+
auto orange13 = createEntity("orange", pear12);
265+
auto orange14 = createEntity("orange", orange13);
266+
auto pear15 = createEntity("pear", grape11);
267+
268+
auto checkEntities = [&ecm](const std::string &_scopedName,
269+
Entity _relativeTo, const std::unordered_set<Entity> &_result,
270+
const std::string &_delim)
271+
{
272+
auto res = gazebo::entitiesFromScopedName(_scopedName, ecm, _relativeTo,
273+
_delim);
274+
EXPECT_EQ(_result.size(), res.size()) << _scopedName;
275+
276+
for (auto it : _result)
277+
{
278+
EXPECT_NE(res.find(it), res.end()) << it << " " << _scopedName;
279+
}
280+
};
281+
282+
checkEntities("watermelon", kNullEntity, {}, "::");
283+
checkEntities("banana", kNullEntity, {banana1}, "::");
284+
checkEntities("orange", kNullEntity, {orange2, orange13, orange14}, ":");
285+
checkEntities("banana::orange", kNullEntity, {orange2}, "::");
286+
checkEntities("banana::grape", kNullEntity, {grape7, grape11}, "::");
287+
checkEntities("grape/pear", kNullEntity, {pear5, pear8, pear12, pear15}, "/");
288+
checkEntities("grape...pear...plum", kNullEntity, {plum6, plum9}, "...");
289+
checkEntities(
290+
"banana::orange::plum::grape::pear::plum", kNullEntity, {plum6}, "::");
291+
checkEntities(
292+
"banana::orange::kiwi::grape::pear::plum", kNullEntity, {}, "::");
293+
checkEntities("orange+orange", kNullEntity, {orange14}, "+");
294+
checkEntities("orange", banana1, {orange2}, "::");
295+
checkEntities("grape", banana1, {grape7, grape11}, "::");
296+
checkEntities("orange", orange2, {}, "::");
297+
checkEntities("orange", orange13, {orange14}, "::");
298+
checkEntities("grape::pear::plum", plum3, {plum6}, "::");
299+
checkEntities("pear", grape11, {pear12, pear15}, "==");
300+
checkEntities("plum=pear", pear8, {pear10}, "=");
301+
}
302+
221303
/////////////////////////////////////////////////
222304
TEST(UtilTest, EntityTypeId)
223305
{

‎src/gui/plugins/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,7 @@ add_subdirectory(entity_tree)
111111
add_subdirectory(grid_config)
112112
add_subdirectory(joint_position_controller)
113113
add_subdirectory(playback_scrubber)
114+
add_subdirectory(plot_3d)
114115
add_subdirectory(resource_spawner)
115116
add_subdirectory(scene3d)
116117
add_subdirectory(shapes)

‎src/gui/plugins/plot_3d/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
gz_add_gui_plugin(Plot3D
2+
SOURCES
3+
Plot3D.cc
4+
QT_HEADERS
5+
Plot3D.hh
6+
TEST_SOURCES
7+
Plot3D_TEST.cc
8+
)

0 commit comments

Comments
 (0)