Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tidy SdfEntityCreator #2179

Closed
wants to merge 5 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 1 addition & 22 deletions include/gz/sim/SdfEntityCreator.hh
Original file line number Diff line number Diff line change
Expand Up @@ -66,27 +66,6 @@ namespace gz
public: explicit SdfEntityCreator(EntityComponentManager &_ecm,
EventManager &_eventManager);

/// \brief Copy constructor
/// \param[in] _creator SdfEntityCreator to copy.
public: SdfEntityCreator(const SdfEntityCreator &_creator);

/// \brief Move constructor
/// \param[in] _creator SdfEntityCreator to move.
public: SdfEntityCreator(SdfEntityCreator &&_creator) noexcept;

/// \brief Move assignment operator.
/// \param[in] _creator SdfEntityCreator component to move.
/// \return Reference to this.
public: SdfEntityCreator &operator=(SdfEntityCreator &&_creator) noexcept;

/// \brief Copy assignment operator.
/// \param[in] _creator SdfEntityCreator to copy.
/// \return Reference to this.
public: SdfEntityCreator &operator=(const SdfEntityCreator &_creator);

/// \brief Destructor.
public: ~SdfEntityCreator();

/// \brief Create all entities that exist in the sdf::World object and
/// load their plugins.
/// \param[in] _world SDF world object.
Expand Down Expand Up @@ -187,7 +166,7 @@ namespace gz
bool _staticParent);

/// \brief Pointer to private data.
private: std::unique_ptr<SdfEntityCreatorPrivate> dataPtr;
GZ_UTILS_IMPL_PTR(dataPtr)
};
}
}
Expand Down
20 changes: 20 additions & 0 deletions include/gz/sim/System.hh
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,10 @@ namespace gz
/// and components are loaded from the corresponding SDF world, and before
/// simulation begins exectution.
class ISystemConfigure {

/// \brief Destructor
public: virtual ~ISystemConfigure() = default;

/// \brief Configure the system
/// \param[in] _entity The entity this plugin is attached to.
/// \param[in] _sdf The SDF Element associated with this system plugin.
Expand All @@ -108,6 +112,10 @@ namespace gz
/// ISystemConfigureParameters::ConfigureParameters is called after
/// ISystemConfigure::Configure.
class ISystemConfigureParameters {

/// \brief Destructor
public: virtual ~ISystemConfigureParameters() = default;

/// \brief Configure the parameters of the system.
/// \param[in] _registry The parameter registry.
public: virtual void ConfigureParameters(
Expand All @@ -117,27 +125,39 @@ namespace gz


class ISystemReset {
/// \brief Destructor
public: virtual ~ISystemReset () = default;

public: virtual void Reset(const UpdateInfo &_info,
EntityComponentManager &_ecm) = 0;
};

/// \class ISystemPreUpdate ISystem.hh gz/sim/System.hh
/// \brief Interface for a system that uses the PreUpdate phase
class ISystemPreUpdate {
/// \brief Destructor
public: virtual ~ISystemPreUpdate() = default;

public: virtual void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) = 0;
};

/// \class ISystemUpdate ISystem.hh gz/sim/System.hh
/// \brief Interface for a system that uses the Update phase
class ISystemUpdate {
/// \brief Destructor
public: virtual ~ISystemUpdate() = default;

public: virtual void Update(const UpdateInfo &_info,
EntityComponentManager &_ecm) = 0;
};

/// \class ISystemPostUpdate ISystem.hh gz/sim/System.hh
/// \brief Interface for a system that uses the PostUpdate phase
class ISystemPostUpdate{
/// \brief Destructor
public: virtual ~ISystemPostUpdate() = default;

public: virtual void PostUpdate(const UpdateInfo &_info,
const EntityComponentManager &_ecm) = 0;
};
Expand Down
17 changes: 4 additions & 13 deletions include/gz/sim/detail/EntityComponentManager.hh
Original file line number Diff line number Diff line change
Expand Up @@ -42,30 +42,21 @@ inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace traits
{
/// \brief Helper struct to determine if an equality operator is present.
struct TestEqualityOperator
{
};
struct TestEqualityOperator {};
template<typename T>
TestEqualityOperator operator == (const T&, const T&);

/// \brief Type trait that determines if an operator== is defined for `T`.
template<typename T>
struct HasEqualityOperator
{
#if !defined(_MSC_VER)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnonnull"
#endif
enum
{
// False positive codecheck "Using C-style cast"
value = !std::is_same<decltype(*(T*)(0) == *(T*)(0)), TestEqualityOperator>::value // NOLINT
value = !std::is_same<decltype(
std::declval<T>() == std::declval<T>()), TestEqualityOperator>::value
};
#if !defined(_MSC_VER)
#pragma GCC diagnostic pop
#endif
};
}
} // namespace traits

//////////////////////////////////////////////////
/// \brief Helper function to compare two objects of the same type using its
Expand Down
89 changes: 4 additions & 85 deletions src/LevelManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -97,53 +97,11 @@ LevelManager::LevelManager(SimulationRunner *_runner, const bool _useLevels)
/////////////////////////////////////////////////
void LevelManager::ReadLevelPerformerInfo()
{
// \todo(anyone) Use SdfEntityCreator to avoid duplication
this->worldEntity = this->runner->entityCompMgr.CreateEntity();
this->worldEntity =
this->entityCreator->CreateEntities(this->runner->sdfWorld);

// World components
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::World());
this->runner->entityCompMgr.CreateComponent(
this->worldEntity, components::Name(this->runner->sdfWorld->Name()));

this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::Gravity(this->runner->sdfWorld->Gravity()));

auto physics = this->runner->sdfWorld->PhysicsByIndex(0);
if (!physics)
{
physics = this->runner->sdfWorld->PhysicsDefault();
}
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::Physics(*physics));

// Populate physics options that aren't accessible outside the Element()
// See https://github.com/osrf/sdformat/issues/508
if (physics->Element() && physics->Element()->HasElement("dart"))
{
auto dartElem = physics->Element()->GetElement("dart");

if (dartElem->HasElement("collision_detector"))
{
auto collisionDetector =
dartElem->Get<std::string>("collision_detector");

this->runner->entityCompMgr.CreateComponent(worldEntity,
components::PhysicsCollisionDetector(collisionDetector));
}
if (dartElem->HasElement("solver") &&
dartElem->GetElement("solver")->HasElement("solver_type"))
{
auto solver =
dartElem->GetElement("solver")->Get<std::string>("solver_type");

this->runner->entityCompMgr.CreateComponent(worldEntity,
components::PhysicsSolver(solver));
}
}

this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::MagneticField(this->runner->sdfWorld->MagneticField()));
// These elements are populated here rather than in the SdfEntityCreator,
// because the serverConfig is available in this context

this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::PhysicsEnginePlugin(
Expand All @@ -167,41 +125,6 @@ void LevelManager::ReadLevelPerformerInfo()

auto worldElem = this->runner->sdfWorld->Element();

// Create Wind
auto windEntity = this->runner->entityCompMgr.CreateEntity();
this->runner->entityCompMgr.CreateComponent(windEntity, components::Wind());
this->runner->entityCompMgr.CreateComponent(
windEntity, components::WorldLinearVelocity(
this->runner->sdfWorld->WindLinearVelocity()));
// Initially the wind linear velocity is used as the seed velocity
this->runner->entityCompMgr.CreateComponent(
windEntity, components::WorldLinearVelocitySeed(
this->runner->sdfWorld->WindLinearVelocity()));

this->entityCreator->SetParent(windEntity, this->worldEntity);

// scene
if (this->runner->sdfWorld->Scene())
{
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::Scene(*this->runner->sdfWorld->Scene()));
}

// atmosphere
if (this->runner->sdfWorld->Atmosphere())
{
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::Atmosphere(*this->runner->sdfWorld->Atmosphere()));
}

// spherical coordinates
if (this->runner->sdfWorld->SphericalCoordinates())
{
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::SphericalCoordinates(
*this->runner->sdfWorld->SphericalCoordinates()));
}

// TODO(anyone) This should probably go somewhere else as it is a global
// constant.
const std::string kPluginName{"gz::sim"};
Expand Down Expand Up @@ -238,10 +161,6 @@ void LevelManager::ReadLevelPerformerInfo()
// Load world plugins.
this->runner->EventMgr().Emit<events::LoadSdfPlugins>(this->worldEntity,
this->runner->sdfWorld->Plugins());

// Store the world's SDF DOM to be used when saving the world to file
this->runner->entityCompMgr.CreateComponent(
worldEntity, components::WorldSdf(*this->runner->sdfWorld));
}

/////////////////////////////////////////////////
Expand Down
64 changes: 29 additions & 35 deletions src/SdfEntityCreator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,11 @@
#include "gz/sim/components/JointAxis.hh"
#include "gz/sim/components/JointType.hh"
#include "gz/sim/components/LaserRetro.hh"
#include "gz/sim/components/Lidar.hh"
#include "gz/sim/components/Light.hh"
#include "gz/sim/components/LightType.hh"
#include "gz/sim/components/LinearAcceleration.hh"
#include "gz/sim/components/LinearVelocity.hh"
#include "gz/sim/components/LinearVelocitySeed.hh"
#include "gz/sim/components/Link.hh"
#include "gz/sim/components/LogicalCamera.hh"
#include "gz/sim/components/MagneticField.hh"
Expand Down Expand Up @@ -81,10 +81,15 @@
#include "gz/sim/components/Visibility.hh"
#include "gz/sim/components/Visual.hh"
#include "gz/sim/components/WideAngleCamera.hh"
#include "gz/sim/components/Wind.hh"
#include "gz/sim/components/WindMode.hh"
#include "gz/sim/components/World.hh"

class gz::sim::SdfEntityCreatorPrivate

namespace gz::sim
{

class SdfEntityCreator::Implementation
{
/// \brief Pointer to entity component manager. We don't assume ownership.
public: EntityComponentManager *ecm{nullptr};
Expand All @@ -105,13 +110,11 @@ class gz::sim::SdfEntityCreatorPrivate
public: std::map<Entity, sdf::Plugins> newVisuals;
};

using namespace gz;
using namespace sim;

namespace {
/////////////////////////////////////////////////
/// \brief Resolve the pose of an SDF DOM object with respect to its relative_to
/// frame. If that fails, return the raw pose
static math::Pose3d ResolveSdfPose(const sdf::SemanticPose &_semPose)
math::Pose3d ResolveSdfPose(const sdf::SemanticPose &_semPose)
{
math::Pose3d pose;
::sdf::Errors errors = _semPose.Resolve(pose);
Expand All @@ -124,7 +127,7 @@ static math::Pose3d ResolveSdfPose(const sdf::SemanticPose &_semPose)
}

/////////////////////////////////////////////////
static std::optional<sdf::JointAxis> ResolveJointAxis(
std::optional<sdf::JointAxis> ResolveJointAxis(
const sdf::JointAxis &_unresolvedAxis)
{
math::Vector3d axisXyz;
Expand Down Expand Up @@ -156,9 +159,9 @@ static std::optional<sdf::JointAxis> ResolveJointAxis(
/// \param[in] _ecm Entity component manager
/// \return The Entity of the descendent link or kNullEntity if link was not
/// found
static Entity FindDescendentLinkEntityByName(const std::string &_name,
const Entity &_model,
const EntityComponentManager &_ecm)
Entity FindDescendentLinkEntityByName(const std::string &_name,
const Entity &_model,
const EntityComponentManager &_ecm)
{
auto ind = _name.find(sdf::kScopeDelimiter);
std::vector<Entity> candidates;
Expand All @@ -185,40 +188,17 @@ static Entity FindDescendentLinkEntityByName(const std::string &_name,
return candidates.front();
}
}
} // namespace

//////////////////////////////////////////////////
SdfEntityCreator::SdfEntityCreator(EntityComponentManager &_ecm,
EventManager &_eventManager)
: dataPtr(std::make_unique<SdfEntityCreatorPrivate>())
: dataPtr(gz::utils::MakeImpl<Implementation>())
{
this->dataPtr->ecm = &_ecm;
this->dataPtr->eventManager = &_eventManager;
}

/////////////////////////////////////////////////
SdfEntityCreator::SdfEntityCreator(const SdfEntityCreator &_creator)
: dataPtr(std::make_unique<SdfEntityCreatorPrivate>(*_creator.dataPtr))
{
}

/////////////////////////////////////////////////
SdfEntityCreator::SdfEntityCreator(SdfEntityCreator &&_creator) noexcept
= default;

//////////////////////////////////////////////////
SdfEntityCreator::~SdfEntityCreator() = default;

/////////////////////////////////////////////////
SdfEntityCreator &SdfEntityCreator::operator=(const SdfEntityCreator &_creator)
{
*this->dataPtr = (*_creator.dataPtr);
return *this;
}

/////////////////////////////////////////////////
SdfEntityCreator &SdfEntityCreator::operator=(SdfEntityCreator &&_creator)
noexcept = default;

//////////////////////////////////////////////////
Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
{
Expand Down Expand Up @@ -326,6 +306,18 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
this->dataPtr->ecm->CreateComponent(worldEntity,
components::MagneticField(_world->MagneticField()));

// Wind
auto windEntity = this->dataPtr->ecm->CreateEntity();
this->dataPtr->ecm->CreateComponent(windEntity, components::Wind());
this->dataPtr->ecm->CreateComponent(
windEntity, components::WorldLinearVelocity(
_world->WindLinearVelocity()));
// Initially the wind linear velocity is used as the seed velocity
this->dataPtr->ecm->CreateComponent(
windEntity, components::WorldLinearVelocitySeed(
_world->WindLinearVelocity()));
this->SetParent(windEntity, worldEntity);

this->dataPtr->eventManager->Emit<events::LoadSdfPlugins>(worldEntity,
_world->Plugins());

Expand Down Expand Up @@ -1085,3 +1077,5 @@ void SdfEntityCreator::SetParent(Entity _child, Entity _parent)
this->dataPtr->ecm->CreateComponent(_child,
components::ParentEntity(_parent));
}

} // namespace gz::sim
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ void CiVctCascadePrivate::UpdateAreaHalfSize(int _axis, float _halfSize)

std::lock_guard<std::mutex> lock(this->serviceMutex);
math::Vector3d areaHalfSize = this->cascade->AreaHalfSize();
areaHalfSize[(size_t)_axis] = static_cast<double>(_halfSize);
areaHalfSize[static_cast<size_t>(_axis)] = static_cast<double>(_halfSize);
this->cascade->SetAreaHalfSize(areaHalfSize);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -796,7 +796,7 @@ void GlobalIlluminationCiVct::PopCascade()
QObject *GlobalIlluminationCiVct::GetCascade(int _idx) const
{
std::lock_guard<std::mutex> lock(this->dataPtr->serviceMutex);
return this->dataPtr->cascades[(size_t)_idx].get();
return this->dataPtr->cascades[static_cast<size_t>(_idx)].get();
}

//////////////////////////////////////////////////
Expand Down
Loading