← Back to branch summary

~j-rivero/+junk/ignition-gazebo

« back to all changes in this revision

Viewing changes to src/SdfEntityCreator.cc

  • Committer: Jose Luis Rivero
  • Date: 2022-02-15 17:35:59 UTC
  • Revision ID: jrivero@osrfoundation.org-20220215173559-qyu3wjmqnrby30k7
Initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/SdfEntityCreator.cc
 
1
/*
 
2
 * Copyright (C) 2019 Open Source Robotics Foundation
 
3
 *
 
4
 * Licensed under the Apache License, Version 2.0 (the "License");
 
5
 * you may not use this file except in compliance with the License.
 
6
 * You may obtain a copy of the License at
 
7
 *
 
8
 *     http://www.apache.org/licenses/LICENSE-2.0
 
9
 *
 
10
 * Unless required by applicable law or agreed to in writing, software
 
11
 * distributed under the License is distributed on an "AS IS" BASIS,
 
12
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 
13
 * See the License for the specific language governing permissions and
 
14
 * limitations under the License.
 
15
 *
 
16
*/
 
17
 
 
18
#include <ignition/common/Console.hh>
 
19
#include <ignition/common/Profiler.hh>
 
20
#include <sdf/Types.hh>
 
21
 
 
22
#include "ignition/gazebo/Events.hh"
 
23
#include "ignition/gazebo/SdfEntityCreator.hh"
 
24
 
 
25
#include "ignition/gazebo/components/Actor.hh"
 
26
#include "ignition/gazebo/components/AirPressureSensor.hh"
 
27
#include "ignition/gazebo/components/Altimeter.hh"
 
28
#include "ignition/gazebo/components/AngularVelocity.hh"
 
29
#include "ignition/gazebo/components/Atmosphere.hh"
 
30
#include "ignition/gazebo/components/Camera.hh"
 
31
#include "ignition/gazebo/components/CanonicalLink.hh"
 
32
#include "ignition/gazebo/components/CastShadows.hh"
 
33
#include "ignition/gazebo/components/ChildLinkName.hh"
 
34
#include "ignition/gazebo/components/Collision.hh"
 
35
#include "ignition/gazebo/components/ContactSensor.hh"
 
36
#include "ignition/gazebo/components/CustomSensor.hh"
 
37
#include "ignition/gazebo/components/DepthCamera.hh"
 
38
#include "ignition/gazebo/components/ForceTorque.hh"
 
39
#include "ignition/gazebo/components/Geometry.hh"
 
40
#include "ignition/gazebo/components/GpuLidar.hh"
 
41
#include "ignition/gazebo/components/Gravity.hh"
 
42
#include "ignition/gazebo/components/Imu.hh"
 
43
#include "ignition/gazebo/components/Inertial.hh"
 
44
#include "ignition/gazebo/components/Joint.hh"
 
45
#include "ignition/gazebo/components/JointAxis.hh"
 
46
#include "ignition/gazebo/components/JointType.hh"
 
47
#include "ignition/gazebo/components/LaserRetro.hh"
 
48
#include "ignition/gazebo/components/Lidar.hh"
 
49
#include "ignition/gazebo/components/Light.hh"
 
50
#include "ignition/gazebo/components/LightType.hh"
 
51
#include "ignition/gazebo/components/LinearAcceleration.hh"
 
52
#include "ignition/gazebo/components/LinearVelocity.hh"
 
53
#include "ignition/gazebo/components/Link.hh"
 
54
#include "ignition/gazebo/components/LogicalCamera.hh"
 
55
#include "ignition/gazebo/components/MagneticField.hh"
 
56
#include "ignition/gazebo/components/Magnetometer.hh"
 
57
#include "ignition/gazebo/components/Material.hh"
 
58
#include "ignition/gazebo/components/Model.hh"
 
59
#include "ignition/gazebo/components/Name.hh"
 
60
#include "ignition/gazebo/components/NavSat.hh"
 
61
#include "ignition/gazebo/components/ParentLinkName.hh"
 
62
#include "ignition/gazebo/components/ParentEntity.hh"
 
63
#include <ignition/gazebo/components/ParticleEmitter.hh>
 
64
#include "ignition/gazebo/components/Physics.hh"
 
65
#include "ignition/gazebo/components/Pose.hh"
 
66
#include "ignition/gazebo/components/RgbdCamera.hh"
 
67
#include "ignition/gazebo/components/Scene.hh"
 
68
#include "ignition/gazebo/components/SegmentationCamera.hh"
 
69
#include "ignition/gazebo/components/SelfCollide.hh"
 
70
#include "ignition/gazebo/components/Sensor.hh"
 
71
#include "ignition/gazebo/components/SourceFilePath.hh"
 
72
#include "ignition/gazebo/components/SphericalCoordinates.hh"
 
73
#include "ignition/gazebo/components/Static.hh"
 
74
#include "ignition/gazebo/components/ThermalCamera.hh"
 
75
#include "ignition/gazebo/components/ThreadPitch.hh"
 
76
#include "ignition/gazebo/components/Transparency.hh"
 
77
#include "ignition/gazebo/components/Visibility.hh"
 
78
#include "ignition/gazebo/components/Visual.hh"
 
79
#include "ignition/gazebo/components/WindMode.hh"
 
80
#include "ignition/gazebo/components/World.hh"
 
81
 
 
82
class ignition::gazebo::SdfEntityCreatorPrivate
 
83
{
 
84
  /// \brief Pointer to entity component manager. We don't assume ownership.
 
85
  public: EntityComponentManager *ecm{nullptr};
 
86
 
 
87
  /// \brief Pointer to event manager. We don't assume ownership.
 
88
  public: EventManager *eventManager{nullptr};
 
89
 
 
90
  /// \brief Keep track of new sensors being added, so we load their plugins
 
91
  /// only after we have their scoped name.
 
92
  public: std::map<Entity, sdf::ElementPtr> newSensors;
 
93
 
 
94
  /// \brief Keep track of new models being added, so we load their plugins
 
95
  /// only after we have their scoped name.
 
96
  public: std::map<Entity, sdf::ElementPtr> newModels;
 
97
 
 
98
  /// \brief Keep track of new visuals being added, so we load their plugins
 
99
  /// only after we have their scoped name.
 
100
  public: std::map<Entity, sdf::ElementPtr> newVisuals;
 
101
};
 
102
 
 
103
using namespace ignition;
 
104
using namespace gazebo;
 
105
 
 
106
/////////////////////////////////////////////////
 
107
/// \brief Resolve the pose of an SDF DOM object with respect to its relative_to
 
108
/// frame. If that fails, return the raw pose
 
109
static math::Pose3d ResolveSdfPose(const sdf::SemanticPose &_semPose)
 
110
{
 
111
  math::Pose3d pose;
 
112
  ::sdf::Errors errors = _semPose.Resolve(pose);
 
113
  if (!errors.empty())
 
114
  {
 
115
    pose = _semPose.RawPose();
 
116
  }
 
117
 
 
118
  return pose;
 
119
}
 
120
 
 
121
/////////////////////////////////////////////////
 
122
static std::optional<sdf::JointAxis> ResolveJointAxis(
 
123
    const sdf::JointAxis &_unresolvedAxis)
 
124
{
 
125
  math::Vector3d axisXyz;
 
126
  const sdf::Errors resolveAxisErrors = _unresolvedAxis.ResolveXyz(axisXyz);
 
127
  if (!resolveAxisErrors.empty())
 
128
  {
 
129
    ignerr << "Failed to resolve axis" << std::endl;
 
130
    return std::nullopt;
 
131
  }
 
132
 
 
133
  sdf::JointAxis resolvedAxis = _unresolvedAxis;
 
134
 
 
135
  const sdf::Errors setXyzErrors = resolvedAxis.SetXyz(axisXyz);
 
136
  if (!setXyzErrors.empty())
 
137
  {
 
138
    ignerr << "Failed to resolve axis" << std::endl;
 
139
    return std::nullopt;
 
140
  }
 
141
 
 
142
  resolvedAxis.SetXyzExpressedIn("");
 
143
  return resolvedAxis;
 
144
}
 
145
 
 
146
//////////////////////////////////////////////////
 
147
/// \brief Find a descendent child link entity by name.
 
148
/// \param[in] _name The relative name of the link with "::" as the scope
 
149
/// delimiter
 
150
/// \param[in] _model Model entity that defines the scope
 
151
/// \param[in] _ecm Entity component manager
 
152
/// \return The Entity of the descendent link or kNullEntity if link was not
 
153
/// found
 
154
static Entity FindDescendentLinkEntityByName(const std::string &_name,
 
155
                                             const Entity &_model,
 
156
                                             const EntityComponentManager &_ecm)
 
157
{
 
158
  auto ind = _name.find(sdf::kSdfScopeDelimiter);
 
159
  std::vector<Entity> candidates;
 
160
  if (ind != std::string::npos)
 
161
  {
 
162
    candidates = _ecm.ChildrenByComponents(
 
163
        _model, components::Model(), components::Name(_name.substr(0, ind)));
 
164
    if (candidates.size() != 1 || (ind + 2 >= _name.size()))
 
165
    {
 
166
      return kNullEntity;
 
167
    }
 
168
    return FindDescendentLinkEntityByName(_name.substr(ind + 2),
 
169
                                          candidates.front(), _ecm);
 
170
  }
 
171
  else
 
172
  {
 
173
    candidates = _ecm.ChildrenByComponents(_model, components::Link(),
 
174
                                           components::Name(_name));
 
175
 
 
176
    if (candidates.size() != 1)
 
177
    {
 
178
      return kNullEntity;
 
179
    }
 
180
    return candidates.front();
 
181
  }
 
182
}
 
183
 
 
184
//////////////////////////////////////////////////
 
185
SdfEntityCreator::SdfEntityCreator(EntityComponentManager &_ecm,
 
186
          EventManager &_eventManager)
 
187
  : dataPtr(std::make_unique<SdfEntityCreatorPrivate>())
 
188
{
 
189
  this->dataPtr->ecm = &_ecm;
 
190
  this->dataPtr->eventManager = &_eventManager;
 
191
}
 
192
 
 
193
/////////////////////////////////////////////////
 
194
SdfEntityCreator::SdfEntityCreator(const SdfEntityCreator &_creator)
 
195
  : dataPtr(std::make_unique<SdfEntityCreatorPrivate>(*_creator.dataPtr))
 
196
{
 
197
}
 
198
 
 
199
/////////////////////////////////////////////////
 
200
SdfEntityCreator::SdfEntityCreator(SdfEntityCreator &&_creator) noexcept
 
201
    = default;
 
202
 
 
203
//////////////////////////////////////////////////
 
204
SdfEntityCreator::~SdfEntityCreator() = default;
 
205
 
 
206
/////////////////////////////////////////////////
 
207
SdfEntityCreator &SdfEntityCreator::operator=(const SdfEntityCreator &_creator)
 
208
{
 
209
  *this->dataPtr = (*_creator.dataPtr);
 
210
  return *this;
 
211
}
 
212
 
 
213
/////////////////////////////////////////////////
 
214
SdfEntityCreator &SdfEntityCreator::operator=(SdfEntityCreator &&_creator)
 
215
    noexcept = default;
 
216
 
 
217
//////////////////////////////////////////////////
 
218
Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
 
219
{
 
220
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)");
 
221
 
 
222
  // World entity
 
223
  Entity worldEntity = this->dataPtr->ecm->CreateEntity();
 
224
 
 
225
  // World components
 
226
  this->dataPtr->ecm->CreateComponent(worldEntity, components::World());
 
227
  this->dataPtr->ecm->CreateComponent(worldEntity,
 
228
      components::Name(_world->Name()));
 
229
 
 
230
  // scene
 
231
  if (_world->Scene())
 
232
  {
 
233
    this->dataPtr->ecm->CreateComponent(worldEntity,
 
234
        components::Scene(*_world->Scene()));
 
235
  }
 
236
 
 
237
  // atmosphere
 
238
  if (_world->Atmosphere())
 
239
  {
 
240
    this->dataPtr->ecm->CreateComponent(worldEntity,
 
241
        components::Atmosphere(*_world->Atmosphere()));
 
242
  }
 
243
 
 
244
  // spherical coordinates
 
245
  if (_world->SphericalCoordinates())
 
246
  {
 
247
    this->dataPtr->ecm->CreateComponent(worldEntity,
 
248
        components::SphericalCoordinates(*_world->SphericalCoordinates()));
 
249
  }
 
250
 
 
251
  // Models
 
252
  for (uint64_t modelIndex = 0; modelIndex < _world->ModelCount();
 
253
      ++modelIndex)
 
254
  {
 
255
    auto model = _world->ModelByIndex(modelIndex);
 
256
    auto modelEntity = this->CreateEntities(model);
 
257
 
 
258
    this->SetParent(modelEntity, worldEntity);
 
259
  }
 
260
 
 
261
  // Actors
 
262
  for (uint64_t actorIndex = 0; actorIndex < _world->ActorCount();
 
263
      ++actorIndex)
 
264
  {
 
265
    auto actor = _world->ActorByIndex(actorIndex);
 
266
    auto actorEntity = this->CreateEntities(actor);
 
267
 
 
268
    this->SetParent(actorEntity, worldEntity);
 
269
  }
 
270
 
 
271
  // Lights
 
272
  for (uint64_t lightIndex = 0; lightIndex < _world->LightCount();
 
273
      ++lightIndex)
 
274
  {
 
275
    auto light = _world->LightByIndex(lightIndex);
 
276
    auto lightEntity = this->CreateEntities(light);
 
277
 
 
278
    this->SetParent(lightEntity, worldEntity);
 
279
  }
 
280
 
 
281
  // Gravity
 
282
  this->dataPtr->ecm->CreateComponent(worldEntity,
 
283
      components::Gravity(_world->Gravity()));
 
284
 
 
285
  // Physics
 
286
  // \todo(anyone) Support picking a specific physics profile
 
287
  auto physics = _world->PhysicsByIndex(0);
 
288
  if (!physics)
 
289
  {
 
290
    physics = _world->PhysicsDefault();
 
291
  }
 
292
  this->dataPtr->ecm->CreateComponent(worldEntity,
 
293
      components::Physics(*physics));
 
294
 
 
295
  // Populate physics options that aren't accessible outside the Element()
 
296
  // See https://github.com/osrf/sdformat/issues/508
 
297
  if (physics->Element() && physics->Element()->HasElement("dart"))
 
298
  {
 
299
    auto dartElem = physics->Element()->GetElement("dart");
 
300
 
 
301
    if (dartElem->HasElement("collision_detector"))
 
302
    {
 
303
      auto collisionDetector =
 
304
          dartElem->Get<std::string>("collision_detector");
 
305
 
 
306
      this->dataPtr->ecm->CreateComponent(worldEntity,
 
307
          components::PhysicsCollisionDetector(collisionDetector));
 
308
    }
 
309
    if (dartElem->HasElement("solver") &&
 
310
        dartElem->GetElement("solver")->HasElement("solver_type"))
 
311
    {
 
312
      auto solver =
 
313
          dartElem->GetElement("solver")->Get<std::string>("solver_type");
 
314
 
 
315
      this->dataPtr->ecm->CreateComponent(worldEntity,
 
316
          components::PhysicsSolver(solver));
 
317
    }
 
318
  }
 
319
 
 
320
  // MagneticField
 
321
  this->dataPtr->ecm->CreateComponent(worldEntity,
 
322
      components::MagneticField(_world->MagneticField()));
 
323
 
 
324
  this->dataPtr->eventManager->Emit<events::LoadPlugins>(worldEntity,
 
325
      _world->Element());
 
326
 
 
327
  // Store the world's SDF DOM to be used when saving the world to file
 
328
  this->dataPtr->ecm->CreateComponent(
 
329
      worldEntity, components::WorldSdf(*_world));
 
330
 
 
331
  return worldEntity;
 
332
}
 
333
 
 
334
//////////////////////////////////////////////////
 
335
Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
 
336
{
 
337
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Model)");
 
338
 
 
339
  auto ent = this->CreateEntities(_model, false);
 
340
 
 
341
  // Load all model plugins afterwards, so we get scoped name for nested models.
 
342
  for (const auto &[entity, element] : this->dataPtr->newModels)
 
343
  {
 
344
    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
 
345
  }
 
346
  this->dataPtr->newModels.clear();
 
347
 
 
348
  // Load sensor plugins after model, so we get scoped name.
 
349
  for (const auto &[entity, element] : this->dataPtr->newSensors)
 
350
  {
 
351
    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
 
352
  }
 
353
  this->dataPtr->newSensors.clear();
 
354
 
 
355
  // Load visual plugins after model, so we get scoped name.
 
356
  for (const auto &[entity, element] : this->dataPtr->newVisuals)
 
357
  {
 
358
    this->dataPtr->eventManager->Emit<events::LoadPlugins>(entity, element);
 
359
  }
 
360
  this->dataPtr->newVisuals.clear();
 
361
 
 
362
  return ent;
 
363
}
 
364
 
 
365
//////////////////////////////////////////////////
 
366
Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
 
367
                                        bool _staticParent)
 
368
{
 
369
  // Entity
 
370
  Entity modelEntity = this->dataPtr->ecm->CreateEntity();
 
371
 
 
372
  // Components
 
373
  this->dataPtr->ecm->CreateComponent(modelEntity, components::Model());
 
374
  this->dataPtr->ecm->CreateComponent(modelEntity,
 
375
      components::Pose(ResolveSdfPose(_model->SemanticPose())));
 
376
  this->dataPtr->ecm->CreateComponent(modelEntity,
 
377
      components::Name(_model->Name()));
 
378
  bool isStatic = _model->Static() || _staticParent;
 
379
  this->dataPtr->ecm->CreateComponent(modelEntity,
 
380
      components::Static(isStatic));
 
381
  this->dataPtr->ecm->CreateComponent(
 
382
      modelEntity, components::WindMode(_model->EnableWind()));
 
383
  this->dataPtr->ecm->CreateComponent(
 
384
      modelEntity, components::SelfCollide(_model->SelfCollide()));
 
385
  this->dataPtr->ecm->CreateComponent(
 
386
      modelEntity, components::SourceFilePath(_model->Element()->FilePath()));
 
387
 
 
388
  // NOTE: Pose components of links, visuals, and collisions are expressed in
 
389
  // the parent frame until we get frames working.
 
390
 
 
391
  // Links
 
392
  const auto *canonicalLink = _model->CanonicalLink();
 
393
 
 
394
  for (uint64_t linkIndex = 0; linkIndex < _model->LinkCount();
 
395
      ++linkIndex)
 
396
  {
 
397
    auto link = _model->LinkByIndex(linkIndex);
 
398
    auto linkEntity = this->CreateEntities(link);
 
399
 
 
400
    this->SetParent(linkEntity, modelEntity);
 
401
 
 
402
    if (canonicalLink == link)
 
403
    {
 
404
      this->dataPtr->ecm->CreateComponent(linkEntity,
 
405
          components::CanonicalLink());
 
406
    }
 
407
 
 
408
    // Set wind mode if the link didn't override it
 
409
    if (!this->dataPtr->ecm->Component<components::WindMode>(linkEntity))
 
410
    {
 
411
      this->dataPtr->ecm->CreateComponent(
 
412
          linkEntity, components::WindMode(_model->EnableWind()));
 
413
    }
 
414
  }
 
415
 
 
416
  // Joints
 
417
  for (uint64_t jointIndex = 0; jointIndex < _model->JointCount();
 
418
      ++jointIndex)
 
419
  {
 
420
    auto joint = _model->JointByIndex(jointIndex);
 
421
    auto jointEntity = this->CreateEntities(joint);
 
422
 
 
423
    this->SetParent(jointEntity, modelEntity);
 
424
  }
 
425
 
 
426
  // Nested Models
 
427
  for (uint64_t modelIndex = 0; modelIndex < _model->ModelCount();
 
428
      ++modelIndex)
 
429
  {
 
430
    auto nestedModel = _model->ModelByIndex(modelIndex);
 
431
    auto nestedModelEntity = this->CreateEntities(nestedModel, isStatic);
 
432
 
 
433
    this->SetParent(nestedModelEntity, modelEntity);
 
434
  }
 
435
 
 
436
  // Find canonical link
 
437
  const auto canonicalLinkPair = _model->CanonicalLinkAndRelativeName();
 
438
  if (canonicalLinkPair.first)
 
439
  {
 
440
    Entity canonicalLinkEntity = FindDescendentLinkEntityByName(
 
441
        canonicalLinkPair.second, modelEntity, *this->dataPtr->ecm);
 
442
    if (kNullEntity != canonicalLinkEntity)
 
443
    {
 
444
      this->dataPtr->ecm->CreateComponent(
 
445
          modelEntity, components::ModelCanonicalLink(canonicalLinkEntity));
 
446
    }
 
447
    else
 
448
    {
 
449
      ignerr << "Could not find the canonical link entity for "
 
450
             << canonicalLinkPair.second << "\n";
 
451
    }
 
452
  }
 
453
  else if(!isStatic)
 
454
  {
 
455
    ignerr << "Could not resolve the canonical link for " << _model->Name()
 
456
           << "\n";
 
457
  }
 
458
 
 
459
  // Store the model's SDF DOM to be used when saving the world to file
 
460
  this->dataPtr->ecm->CreateComponent(
 
461
      modelEntity, components::ModelSdf(*_model));
 
462
 
 
463
  // Keep track of models so we can load their plugins after loading the entire
 
464
  // model and having its full scoped name.
 
465
  this->dataPtr->newModels[modelEntity] = _model->Element();
 
466
 
 
467
  return modelEntity;
 
468
}
 
469
 
 
470
//////////////////////////////////////////////////
 
471
Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor)
 
472
{
 
473
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Actor)");
 
474
 
 
475
  // Entity
 
476
  Entity actorEntity = this->dataPtr->ecm->CreateEntity();
 
477
 
 
478
  // Components
 
479
  this->dataPtr->ecm->CreateComponent(actorEntity, components::Actor(*_actor));
 
480
  this->dataPtr->ecm->CreateComponent(actorEntity,
 
481
      components::Pose(_actor->RawPose()));
 
482
  this->dataPtr->ecm->CreateComponent(actorEntity,
 
483
      components::Name(_actor->Name()));
 
484
 
 
485
  // Actor plugins
 
486
  this->dataPtr->eventManager->Emit<events::LoadPlugins>(actorEntity,
 
487
      _actor->Element());
 
488
 
 
489
  return actorEntity;
 
490
}
 
491
 
 
492
//////////////////////////////////////////////////
 
493
Entity SdfEntityCreator::CreateEntities(const sdf::Light *_light)
 
494
{
 
495
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Light)");
 
496
 
 
497
  // Entity
 
498
  Entity lightEntity = this->dataPtr->ecm->CreateEntity();
 
499
 
 
500
  // Components
 
501
  this->dataPtr->ecm->CreateComponent(lightEntity, components::Light(*_light));
 
502
  this->dataPtr->ecm->CreateComponent(lightEntity,
 
503
      components::Pose(ResolveSdfPose(_light->SemanticPose())));
 
504
  this->dataPtr->ecm->CreateComponent(lightEntity,
 
505
      components::Name(_light->Name()));
 
506
 
 
507
  this->dataPtr->ecm->CreateComponent(lightEntity,
 
508
    components::LightType(convert(_light->Type())));
 
509
 
 
510
  return lightEntity;
 
511
}
 
512
 
 
513
//////////////////////////////////////////////////
 
514
Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link)
 
515
{
 
516
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Link)");
 
517
 
 
518
  // Entity
 
519
  Entity linkEntity = this->dataPtr->ecm->CreateEntity();
 
520
 
 
521
  // Components
 
522
  this->dataPtr->ecm->CreateComponent(linkEntity, components::Link());
 
523
 
 
524
  this->dataPtr->ecm->CreateComponent(linkEntity,
 
525
      components::Pose(ResolveSdfPose(_link->SemanticPose())));
 
526
  this->dataPtr->ecm->CreateComponent(linkEntity,
 
527
      components::Name(_link->Name()));
 
528
  this->dataPtr->ecm->CreateComponent(linkEntity,
 
529
      components::Inertial(_link->Inertial()));
 
530
 
 
531
  if (_link->EnableWind())
 
532
  {
 
533
    this->dataPtr->ecm->CreateComponent(
 
534
        linkEntity, components::WindMode(_link->EnableWind()));
 
535
  }
 
536
 
 
537
  // Visuals
 
538
  for (uint64_t visualIndex = 0; visualIndex < _link->VisualCount();
 
539
      ++visualIndex)
 
540
  {
 
541
    auto visual = _link->VisualByIndex(visualIndex);
 
542
    auto visualEntity = this->CreateEntities(visual);
 
543
 
 
544
    this->SetParent(visualEntity, linkEntity);
 
545
  }
 
546
 
 
547
  // Collisions
 
548
  for (uint64_t collisionIndex = 0; collisionIndex < _link->CollisionCount();
 
549
      ++collisionIndex)
 
550
  {
 
551
    auto collision = _link->CollisionByIndex(collisionIndex);
 
552
    auto collisionEntity = this->CreateEntities(collision);
 
553
 
 
554
    this->SetParent(collisionEntity, linkEntity);
 
555
  }
 
556
 
 
557
  // Lights
 
558
  for (uint64_t lightIndex = 0; lightIndex < _link->LightCount();
 
559
      ++lightIndex)
 
560
  {
 
561
    auto light = _link->LightByIndex(lightIndex);
 
562
    auto lightEntity = this->CreateEntities(light);
 
563
 
 
564
    this->SetParent(lightEntity, linkEntity);
 
565
  }
 
566
 
 
567
  // Sensors
 
568
  for (uint64_t sensorIndex = 0; sensorIndex < _link->SensorCount();
 
569
      ++sensorIndex)
 
570
  {
 
571
    auto sensor = _link->SensorByIndex(sensorIndex);
 
572
    auto sensorEntity = this->CreateEntities(sensor);
 
573
 
 
574
    this->SetParent(sensorEntity, linkEntity);
 
575
  }
 
576
 
 
577
  // Particle emitters
 
578
  for (uint64_t emitterIndex = 0; emitterIndex  < _link->ParticleEmitterCount();
 
579
       ++emitterIndex)
 
580
  {
 
581
    auto emitter = _link->ParticleEmitterByIndex(emitterIndex);
 
582
    auto emitterEntity = this->CreateEntities(emitter);
 
583
 
 
584
    this->SetParent(emitterEntity, linkEntity);
 
585
  }
 
586
 
 
587
  return linkEntity;
 
588
}
 
589
 
 
590
//////////////////////////////////////////////////
 
591
Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
 
592
{
 
593
  return this->CreateEntities(_joint, false);
 
594
}
 
595
 
 
596
//////////////////////////////////////////////////
 
597
Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
 
598
    bool _resolved)
 
599
{
 
600
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)");
 
601
 
 
602
  // Entity
 
603
  Entity jointEntity = this->dataPtr->ecm->CreateEntity();
 
604
 
 
605
  // Components
 
606
  this->dataPtr->ecm->CreateComponent(jointEntity,
 
607
      components::Joint());
 
608
  this->dataPtr->ecm->CreateComponent(jointEntity,
 
609
      components::JointType(_joint->Type()));
 
610
 
 
611
  // Sensors
 
612
  for (uint64_t sensorIndex = 0; sensorIndex < _joint->SensorCount();
 
613
      ++sensorIndex)
 
614
  {
 
615
    auto sensor = _joint->SensorByIndex(sensorIndex);
 
616
    auto sensorEntity = this->CreateEntities(sensor);
 
617
 
 
618
    this->SetParent(sensorEntity, jointEntity);
 
619
  }
 
620
 
 
621
  if (_joint->Axis(0))
 
622
  {
 
623
    auto resolvedAxis = ResolveJointAxis(*_joint->Axis(0));
 
624
    if (!resolvedAxis)
 
625
    {
 
626
      ignerr << "Failed to resolve joint axis 0 for joint '" << _joint->Name()
 
627
             << "'" << std::endl;
 
628
      return kNullEntity;
 
629
    }
 
630
 
 
631
    this->dataPtr->ecm->CreateComponent(jointEntity,
 
632
        components::JointAxis(std::move(*resolvedAxis)));
 
633
  }
 
634
 
 
635
  if (_joint->Axis(1))
 
636
  {
 
637
    auto resolvedAxis = ResolveJointAxis(*_joint->Axis(1));
 
638
    if (!resolvedAxis)
 
639
    {
 
640
      ignerr << "Failed to resolve joint axis 1 for joint '" << _joint->Name()
 
641
             << "'" << std::endl;
 
642
      return kNullEntity;
 
643
    }
 
644
 
 
645
    this->dataPtr->ecm->CreateComponent(jointEntity,
 
646
        components::JointAxis2(std::move(*resolvedAxis)));
 
647
  }
 
648
 
 
649
  this->dataPtr->ecm->CreateComponent(jointEntity,
 
650
      components::Pose(ResolveSdfPose(_joint->SemanticPose())));
 
651
  this->dataPtr->ecm->CreateComponent(jointEntity ,
 
652
      components::Name(_joint->Name()));
 
653
  this->dataPtr->ecm->CreateComponent(jointEntity ,
 
654
      components::ThreadPitch(_joint->ThreadPitch()));
 
655
 
 
656
 
 
657
  std::string resolvedParentLinkName;
 
658
  if (_resolved)
 
659
  {
 
660
    resolvedParentLinkName = _joint->ParentLinkName();
 
661
  }
 
662
  else
 
663
  {
 
664
 
 
665
    const auto resolveParentErrors =
 
666
      _joint->ResolveParentLink(resolvedParentLinkName);
 
667
    if (!resolveParentErrors.empty())
 
668
    {
 
669
      ignerr << "Failed to resolve parent link for joint '" << _joint->Name()
 
670
             << "' with parent name '" << _joint->ParentLinkName() << "'"
 
671
             << std::endl;
 
672
      for (const auto &error : resolveParentErrors)
 
673
      {
 
674
        ignerr << error << std::endl;
 
675
      }
 
676
 
 
677
      return kNullEntity;
 
678
    }
 
679
  }
 
680
  this->dataPtr->ecm->CreateComponent(
 
681
      jointEntity, components::ParentLinkName(resolvedParentLinkName));
 
682
 
 
683
  std::string resolvedChildLinkName;
 
684
  if (_resolved)
 
685
  {
 
686
    resolvedChildLinkName = _joint->ChildLinkName();
 
687
  }
 
688
  else
 
689
  {
 
690
    const auto resolveChildErrors =
 
691
      _joint->ResolveChildLink(resolvedChildLinkName);
 
692
    if (!resolveChildErrors.empty())
 
693
    {
 
694
      ignerr << "Failed to resolve child link for joint '" << _joint->Name()
 
695
             << "' with child name '" << _joint->ChildLinkName() << "'"
 
696
             << std::endl;
 
697
      for (const auto &error : resolveChildErrors)
 
698
      {
 
699
        ignerr << error << std::endl;
 
700
      }
 
701
 
 
702
      return kNullEntity;
 
703
    }
 
704
  }
 
705
 
 
706
  this->dataPtr->ecm->CreateComponent(
 
707
      jointEntity, components::ChildLinkName(resolvedChildLinkName));
 
708
 
 
709
  return jointEntity;
 
710
}
 
711
 
 
712
//////////////////////////////////////////////////
 
713
Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual)
 
714
{
 
715
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Visual)");
 
716
 
 
717
  // Entity
 
718
  Entity visualEntity = this->dataPtr->ecm->CreateEntity();
 
719
 
 
720
  // Components
 
721
  this->dataPtr->ecm->CreateComponent(visualEntity, components::Visual());
 
722
  this->dataPtr->ecm->CreateComponent(visualEntity,
 
723
      components::Pose(ResolveSdfPose(_visual->SemanticPose())));
 
724
  this->dataPtr->ecm->CreateComponent(visualEntity,
 
725
      components::Name(_visual->Name()));
 
726
  this->dataPtr->ecm->CreateComponent(visualEntity,
 
727
      components::CastShadows(_visual->CastShadows()));
 
728
  this->dataPtr->ecm->CreateComponent(visualEntity,
 
729
      components::Transparency(_visual->Transparency()));
 
730
  this->dataPtr->ecm->CreateComponent(visualEntity,
 
731
      components::VisibilityFlags(_visual->VisibilityFlags()));
 
732
 
 
733
  if (_visual->HasLaserRetro())
 
734
  {
 
735
    this->dataPtr->ecm->CreateComponent(visualEntity,
 
736
        components::LaserRetro(_visual->LaserRetro()));
 
737
  }
 
738
 
 
739
  if (_visual->Geom())
 
740
  {
 
741
    this->dataPtr->ecm->CreateComponent(visualEntity,
 
742
        components::Geometry(*_visual->Geom()));
 
743
  }
 
744
 
 
745
  // \todo(louise) Populate with default material if undefined
 
746
  if (_visual->Material())
 
747
  {
 
748
    this->dataPtr->ecm->CreateComponent(visualEntity,
 
749
        components::Material(*_visual->Material()));
 
750
  }
 
751
 
 
752
  // Keep track of visuals so we can load their plugins after loading the
 
753
  // entire model and having its full scoped name.
 
754
  this->dataPtr->newVisuals[visualEntity] = _visual->Element();
 
755
 
 
756
  return visualEntity;
 
757
}
 
758
 
 
759
//////////////////////////////////////////////////
 
760
Entity SdfEntityCreator::CreateEntities(const sdf::ParticleEmitter *_emitter)
 
761
{
 
762
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::ParticleEmitter)");
 
763
 
 
764
  // Entity
 
765
  Entity emitterEntity = this->dataPtr->ecm->CreateEntity();
 
766
 
 
767
  // Components
 
768
  this->dataPtr->ecm->CreateComponent(emitterEntity,
 
769
      components::ParticleEmitter(convert<msgs::ParticleEmitter>(*_emitter)));
 
770
  this->dataPtr->ecm->CreateComponent(emitterEntity,
 
771
      components::Pose(ResolveSdfPose(_emitter->SemanticPose())));
 
772
  this->dataPtr->ecm->CreateComponent(emitterEntity,
 
773
      components::Name(_emitter->Name()));
 
774
 
 
775
  return emitterEntity;
 
776
}
 
777
 
 
778
//////////////////////////////////////////////////
 
779
Entity SdfEntityCreator::CreateEntities(const sdf::Collision *_collision)
 
780
{
 
781
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Collision)");
 
782
 
 
783
  // Entity
 
784
  Entity collisionEntity = this->dataPtr->ecm->CreateEntity();
 
785
 
 
786
  // Components
 
787
  this->dataPtr->ecm->CreateComponent(collisionEntity,
 
788
      components::Collision());
 
789
  this->dataPtr->ecm->CreateComponent(collisionEntity,
 
790
      components::Pose(ResolveSdfPose(_collision->SemanticPose())));
 
791
  this->dataPtr->ecm->CreateComponent(collisionEntity,
 
792
      components::Name(_collision->Name()));
 
793
 
 
794
  if (_collision->Geom())
 
795
  {
 
796
    this->dataPtr->ecm->CreateComponent(collisionEntity,
 
797
        components::Geometry(*_collision->Geom()));
 
798
  }
 
799
 
 
800
  this->dataPtr->ecm->CreateComponent(collisionEntity,
 
801
      components::CollisionElement(*_collision));
 
802
 
 
803
  return collisionEntity;
 
804
}
 
805
 
 
806
//////////////////////////////////////////////////
 
807
Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor)
 
808
{
 
809
  IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Sensor)");
 
810
 
 
811
  // Entity
 
812
  Entity sensorEntity = this->dataPtr->ecm->CreateEntity();
 
813
 
 
814
  // Components
 
815
  this->dataPtr->ecm->CreateComponent(sensorEntity,
 
816
      components::Sensor());
 
817
  this->dataPtr->ecm->CreateComponent(sensorEntity,
 
818
      components::Pose(ResolveSdfPose(_sensor->SemanticPose())));
 
819
  this->dataPtr->ecm->CreateComponent(sensorEntity,
 
820
      components::Name(_sensor->Name()));
 
821
 
 
822
  if (_sensor->Type() == sdf::SensorType::CAMERA)
 
823
  {
 
824
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
825
        components::Camera(*_sensor));
 
826
  }
 
827
  else if (_sensor->Type() == sdf::SensorType::GPU_LIDAR)
 
828
  {
 
829
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
830
        components::GpuLidar(*_sensor));
 
831
  }
 
832
  else if (_sensor->Type() == sdf::SensorType::LIDAR)
 
833
  {
 
834
    // \todo(anyone) Implement CPU-based lidar
 
835
    // this->dataPtr->ecm->CreateComponent(sensorEntity,
 
836
    //     components::Lidar(*_sensor));
 
837
    ignwarn << "Sensor type LIDAR not supported yet. Try using"
 
838
      << "a GPU LIDAR instead." << std::endl;
 
839
  }
 
840
  else if (_sensor->Type() == sdf::SensorType::DEPTH_CAMERA)
 
841
  {
 
842
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
843
        components::DepthCamera(*_sensor));
 
844
  }
 
845
  else if (_sensor->Type() == sdf::SensorType::RGBD_CAMERA)
 
846
  {
 
847
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
848
        components::RgbdCamera(*_sensor));
 
849
  }
 
850
  else if (_sensor->Type() == sdf::SensorType::THERMAL_CAMERA)
 
851
  {
 
852
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
853
        components::ThermalCamera(*_sensor));
 
854
  }
 
855
  else if (_sensor->Type() == sdf::SensorType::SEGMENTATION_CAMERA)
 
856
  {
 
857
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
858
        components::SegmentationCamera(*_sensor));
 
859
  }
 
860
  else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE)
 
861
  {
 
862
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
863
        components::AirPressureSensor(*_sensor));
 
864
 
 
865
    // create components to be filled by physics
 
866
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
867
        components::WorldPose(math::Pose3d::Zero));
 
868
  }
 
869
  else if (_sensor->Type() == sdf::SensorType::ALTIMETER)
 
870
  {
 
871
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
872
        components::Altimeter(*_sensor));
 
873
 
 
874
    // create components to be filled by physics
 
875
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
876
        components::WorldPose(math::Pose3d::Zero));
 
877
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
878
        components::WorldLinearVelocity(math::Vector3d::Zero));
 
879
  }
 
880
  else if (_sensor->Type() == sdf::SensorType::GPS ||
 
881
           _sensor->Type() == sdf::SensorType::NAVSAT)
 
882
  {
 
883
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
884
            components::NavSat(*_sensor));
 
885
 
 
886
    // Create components to be filled by physics.
 
887
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
888
        components::WorldLinearVelocity(math::Vector3d::Zero));
 
889
  }
 
890
  else if (_sensor->Type() == sdf::SensorType::IMU)
 
891
  {
 
892
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
893
            components::Imu(*_sensor));
 
894
 
 
895
    // create components to be filled by physics
 
896
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
897
            components::WorldPose(math::Pose3d::Zero));
 
898
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
899
            components::AngularVelocity(math::Vector3d::Zero));
 
900
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
901
            components::LinearAcceleration(math::Vector3d::Zero));
 
902
  }
 
903
  else if (_sensor->Type() == sdf::SensorType::FORCE_TORQUE)
 
904
  {
 
905
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
906
        components::ForceTorque(*_sensor));
 
907
  }
 
908
  else if (_sensor->Type() == sdf::SensorType::LOGICAL_CAMERA)
 
909
  {
 
910
    auto elem = _sensor->Element();
 
911
 
 
912
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
913
        components::LogicalCamera(elem));
 
914
 
 
915
    // create components to be filled by physics
 
916
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
917
        components::WorldPose(math::Pose3d::Zero));
 
918
  }
 
919
  else if (_sensor->Type() == sdf::SensorType::MAGNETOMETER)
 
920
  {
 
921
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
922
        components::Magnetometer(*_sensor));
 
923
 
 
924
    // create components to be filled by physics
 
925
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
926
        components::WorldPose(math::Pose3d::Zero));
 
927
  }
 
928
  else if (_sensor->Type() == sdf::SensorType::CONTACT)
 
929
  {
 
930
    auto elem = _sensor->Element();
 
931
 
 
932
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
933
            components::ContactSensor(elem));
 
934
    // We will let the contact system create the necessary components for
 
935
    // physics to populate.
 
936
  }
 
937
  else if (_sensor->Type() == sdf::SensorType::CUSTOM)
 
938
  {
 
939
    auto elem = _sensor->Element();
 
940
    this->dataPtr->ecm->CreateComponent(sensorEntity,
 
941
            components::CustomSensor(*_sensor));
 
942
  }
 
943
  else
 
944
  {
 
945
    ignwarn << "Sensor type [" << static_cast<int>(_sensor->Type())
 
946
            << "] not supported yet." << std::endl;
 
947
  }
 
948
 
 
949
  // Keep track of sensors so we can load their plugins after loading the entire
 
950
  // model and having its full scoped name.
 
951
  this->dataPtr->newSensors[sensorEntity] = _sensor->Element();
 
952
 
 
953
  return sensorEntity;
 
954
}
 
955
 
 
956
//////////////////////////////////////////////////
 
957
void SdfEntityCreator::RequestRemoveEntity(Entity _entity, bool _recursive)
 
958
{
 
959
  // Leave children parentless
 
960
  if (!_recursive)
 
961
  {
 
962
    auto childEntities = this->dataPtr->ecm->ChildrenByComponents(_entity,
 
963
        components::ParentEntity(_entity));
 
964
    for (const auto childEntity : childEntities)
 
965
    {
 
966
      this->dataPtr->ecm->RemoveComponent<components::ParentEntity>(
 
967
          childEntity);
 
968
    }
 
969
  }
 
970
 
 
971
  this->dataPtr->ecm->RequestRemoveEntity(_entity, _recursive);
 
972
}
 
973
 
 
974
//////////////////////////////////////////////////
 
975
void SdfEntityCreator::SetParent(Entity _child, Entity _parent)
 
976
{
 
977
  // TODO(louise) Figure out a way to avoid duplication while keeping all
 
978
  // state in components and also keeping a convenient graph in the ECM
 
979
  this->dataPtr->ecm->SetParentEntity(_child, _parent);
 
980
  this->dataPtr->ecm->CreateComponent(_child,
 
981
      components::ParentEntity(_parent));
 
982
}