diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf new file mode 100644 index 0000000000..afa33866c3 --- /dev/null +++ b/examples/worlds/plot_3d.sdf @@ -0,0 +1,152 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 5 -1.5 3 0 0.37 2.8 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + + + + Upper link plot + + Double_pendulum::upper_link + 0 0 1 + 0 0 1 + 20 + 0.1 + + + + + + Lower link plot + + Double_pendulum::lower_link + 0 1 0 + 0 0 1 + 40 + 0.3 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + Double_pendulum + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base + + + + diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 1f6ad18cd6..8d7f228417 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -185,8 +185,9 @@ namespace ignition /// body-fixed frame. If no offset is given, the velocity at the origin of /// the Link frame will be returned. /// \param[in] _ecm Entity-component manager. - /// \return Linear velocity of the link or nullopt if the link does not - /// have components components::WorldPose. + /// \return Linear velocity of the link or nullopt if the velocity checks + /// aren't enabled. + /// \sa EnableVelocityChecks public: std::optional WorldLinearVelocity( const EntityComponentManager &_ecm) const; @@ -195,28 +196,47 @@ namespace ignition /// \param[in] _ecm Entity-component manager. /// \param[in] _offset Offset of the point from the origin of the Link /// frame, expressed in the body-fixed frame. - /// \return Linear velocity of the point on the body or nullopt if the - /// link does not have components components::WorldPose, - /// components::WorldLinearVelocity and components::WorldAngularVelocity. + /// \return Linear velocity of the point on the body or nullopt if + /// velocity checks aren't enabled. + /// \sa EnableVelocityChecks public: std::optional WorldLinearVelocity( const EntityComponentManager &_ecm, const math::Vector3d &_offset) const; /// \brief Get the angular velocity of the link in the world frame /// \param[in] _ecm Entity-component manager. - /// \return Angular velocity of the link or nullopt if the link does not - /// have a components::WorldAngularVelocity component. + /// \return Angular velocity of the link or nullopt if velocity checks + /// aren't enabled. + /// \sa EnableVelocityChecks public: std::optional WorldAngularVelocity( const EntityComponentManager &_ecm) const; + /// \brief By default, Gazebo will not report velocities for a link, so + /// functions like `WorldLinearVelocity` will return nullopt. This + /// function can be used to enable all velocity checks. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableVelocityChecks(EntityComponentManager &_ecm, + bool _enable = true) const; + /// \brief Get the linear acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. /// \return Linear acceleration of the body in the world frame or nullopt - /// if the link does not have a components::WorldLinearAcceleration - /// component. + /// if acceleration checks aren't enabled. + /// \sa EnableAccelerationChecks public: std::optional WorldLinearAcceleration( const EntityComponentManager &_ecm) const; + /// \brief By default, Gazebo will not report accelerations for a link, so + /// functions like `WorldLinearAcceleration` will return nullopt. This + /// function can be used to enable all acceleration checks. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableAccelerationChecks(EntityComponentManager &_ecm, + bool _enable = true) const; + /// \brief Get the inertia matrix in the world frame. /// \param[in] _ecm Entity-component manager. /// \return Inertia matrix in world frame, returns nullopt if link diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 8387399ae3..fb2e7a17ef 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -218,6 +219,16 @@ namespace ignition const SystemPluginPtr &_system, const unsigned int _worldIndex = 0); + /// \brief Add a System to the server. The server must not be running when + /// calling this. + /// \param[in] _system System to be added + /// \param[in] _worldIndex Index of the world to add to. + /// \return Whether the system was added successfully, or std::nullopt + /// if _worldIndex is invalid. + public: std::optional AddSystem( + const std::shared_ptr &_system, + const unsigned int _worldIndex = 0); + /// \brief Get an Entity based on a name. /// \details If multiple entities with the same name exist, the first /// entity found will be returned. diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index b996e2fe03..ef9afc354b 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_UTIL_HH_ #include +#include #include #include @@ -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 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 @@ -101,6 +130,12 @@ namespace ignition Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity, const EntityComponentManager &_ecm); + /// \brief Get the first world entity that's found. + /// \param[in] _ecm Immutable reference to ECM. + /// \return World entity ID. + Entity IGNITION_GAZEBO_VISIBLE worldEntity( + const EntityComponentManager &_ecm); + /// \brief Helper function to remove a parent scope from a given name. /// This removes the first name found before the delimiter. /// \param[in] _name Input name possibly generated by scopedName. @@ -174,6 +209,34 @@ namespace ignition const EntityComponentManager &_ecm, bool _excludeWorld = true); + /// \brief Helper function to "enable" a component (i.e. create it if it + /// doesn't exist) or "disable" a component (i.e. remove it if it exists). + /// \param[in] _ecm Mutable reference to the ECM + /// \param[in] _entity Entity whose component is being enabled + /// \param[in] _enable True to enable (create), false to disable (remove). + /// Defaults to true. + /// \return True if a component was created or removed, false if nothing + /// changed. + template + bool IGNITION_GAZEBO_VISIBLE enableComponent(EntityComponentManager &_ecm, + Entity _entity, bool _enable = true) + { + bool changed{false}; + + auto exists = _ecm.Component(_entity); + if (_enable && !exists) + { + _ecm.CreateComponent(_entity, ComponentType()); + changed = true; + } + else if (!_enable && exists) + { + _ecm.RemoveComponent(_entity); + changed = true; + } + return changed; + } + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 9a8e4c28f5..4eac724b55 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -47,6 +47,7 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject public: ~GuiRunner() override; /// \brief Callback when a plugin has been added. + /// This function has no effect and is left here for ABI compatibility. /// \param[in] _objectName Plugin's object name. public slots: void OnPluginAdded(const QString &_objectName); diff --git a/src/Link.cc b/src/Link.cc index 6289b7909f..41f814cd88 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -32,6 +32,7 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/WindMode.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/Link.hh" @@ -228,6 +229,22 @@ std::optional Link::WorldAngularVelocity( this->dataPtr->id); } +////////////////////////////////////////////////// +void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + ////////////////////////////////////////////////// std::optional Link::WorldLinearAcceleration( const EntityComponentManager &_ecm) const @@ -236,6 +253,16 @@ std::optional Link::WorldLinearAcceleration( this->dataPtr->id); } +////////////////////////////////////////////////// +void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + ////////////////////////////////////////////////// std::optional Link::WorldInertiaMatrix( const EntityComponentManager &_ecm) const diff --git a/src/Server.cc b/src/Server.cc index bb188ef993..7e7de3b975 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -354,6 +354,26 @@ std::optional Server::AddSystem(const SystemPluginPtr &_system, return std::nullopt; } +////////////////////////////////////////////////// +std::optional Server::AddSystem(const std::shared_ptr &_system, + const unsigned int _worldIndex) +{ + std::lock_guard lock(this->dataPtr->runMutex); + if (this->dataPtr->running) + { + ignerr << "Cannot add system while the server is runnnng.\n"; + return false; + } + + if (_worldIndex < this->dataPtr->simRunners.size()) + { + this->dataPtr->simRunners[_worldIndex]->AddSystem(_system); + return true; + } + + return std::nullopt; +} + ////////////////////////////////////////////////// bool Server::HasEntity(const std::string &_name, const unsigned int _worldIndex) const diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index e2ee8eed0f..de31ea43f0 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -635,6 +635,7 @@ TEST_P(ServerFixture, AddSystemWhileRunning) EXPECT_EQ(3u, *server.SystemCount()); + // Add system from plugin gazebo::SystemLoader systemLoader; auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so", "ignition::gazebo::MockSystem", nullptr); @@ -645,6 +646,13 @@ TEST_P(ServerFixture, AddSystemWhileRunning) EXPECT_FALSE(result.value()); EXPECT_EQ(3u, *server.SystemCount()); + // Add system pointer + auto mockSystem = std::make_shared(); + result = server.AddSystem(mockSystem); + ASSERT_TRUE(result.has_value()); + EXPECT_FALSE(result.value()); + EXPECT_EQ(3u, *server.SystemCount()); + // Stop the server std::raise(SIGTERM); @@ -664,28 +672,55 @@ TEST_P(ServerFixture, AddSystemAfterLoad) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); + // Add system from plugin gazebo::SystemLoader systemLoader; auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so", "ignition::gazebo::MockSystem", nullptr); ASSERT_TRUE(mockSystemPlugin.has_value()); + auto system = mockSystemPlugin.value()->QueryInterface(); + EXPECT_NE(system, nullptr); + auto mockSystem = dynamic_cast(system); + ASSERT_NE(mockSystem, nullptr); + EXPECT_EQ(3u, *server.SystemCount()); + EXPECT_EQ(0u, mockSystem->configureCallCount); + EXPECT_TRUE(*server.AddSystem(mockSystemPlugin.value())); + EXPECT_EQ(4u, *server.SystemCount()); + EXPECT_EQ(1u, mockSystem->configureCallCount); - auto system = mockSystemPlugin.value()->QueryInterface(); - EXPECT_NE(system, nullptr); - auto mockSystem = dynamic_cast(system); - EXPECT_NE(mockSystem, nullptr); + // Add system pointer + auto mockSystemLocal = std::make_shared(); + EXPECT_EQ(0u, mockSystemLocal->configureCallCount); + + EXPECT_TRUE(server.AddSystem(mockSystemLocal)); + EXPECT_EQ(5u, *server.SystemCount()); + EXPECT_EQ(1u, mockSystemLocal->configureCallCount); + // Check that update callbacks are called server.SetUpdatePeriod(1us); EXPECT_EQ(0u, mockSystem->preUpdateCallCount); EXPECT_EQ(0u, mockSystem->updateCallCount); EXPECT_EQ(0u, mockSystem->postUpdateCallCount); + EXPECT_EQ(0u, mockSystemLocal->preUpdateCallCount); + EXPECT_EQ(0u, mockSystemLocal->updateCallCount); + EXPECT_EQ(0u, mockSystemLocal->postUpdateCallCount); server.Run(true, 1, false); EXPECT_EQ(1u, mockSystem->preUpdateCallCount); EXPECT_EQ(1u, mockSystem->updateCallCount); EXPECT_EQ(1u, mockSystem->postUpdateCallCount); + EXPECT_EQ(1u, mockSystemLocal->preUpdateCallCount); + EXPECT_EQ(1u, mockSystemLocal->updateCallCount); + EXPECT_EQ(1u, mockSystemLocal->postUpdateCallCount); + + // Add to inexistent world + auto result = server.AddSystem(mockSystemPlugin.value(), 100); + EXPECT_FALSE(result.has_value()); + + result = server.AddSystem(mockSystemLocal, 100); + EXPECT_FALSE(result.has_value()); } ///////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 35c0158098..dd27cd8160 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -433,28 +433,61 @@ void SimulationRunner::PublishStats() this->rootClockPub.Publish(clockMsg); } -///////////////////////////////////////////////// -void SimulationRunner::AddSystem(const SystemPluginPtr &_system) +////////////////////////////////////////////////// +void SimulationRunner::AddSystem(const SystemPluginPtr &_system, + std::optional _entity, + std::optional> _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SimulationRunner::AddSystem( + const std::shared_ptr &_system, + std::optional _entity, + std::optional> _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SimulationRunner::AddSystemImpl( + SystemInternal _system, + std::optional _entity, + std::optional> _sdf) { + // Call configure + if (_system.configure) + { + // Default to world entity and SDF + auto entity = _entity.has_value() ? _entity.value() + : worldEntity(this->entityCompMgr); + auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); + + _system.configure->Configure( + entity, sdf, + this->entityCompMgr, + this->eventMgr); + } + + // Update callbacks will be handled later, add to queue std::lock_guard lock(this->pendingSystemsMutex); this->pendingSystems.push_back(_system); } ///////////////////////////////////////////////// -void SimulationRunner::AddSystemToRunner(const SystemPluginPtr &_system) +void SimulationRunner::AddSystemToRunner(SystemInternal _system) { - this->systems.push_back(SystemInternal(_system)); - - const auto &system = this->systems.back(); + this->systems.push_back(_system); - if (system.preupdate) - this->systemsPreupdate.push_back(system.preupdate); + if (_system.preupdate) + this->systemsPreupdate.push_back(_system.preupdate); - if (system.update) - this->systemsUpdate.push_back(system.update); + if (_system.update) + this->systemsUpdate.push_back(_system.update); - if (system.postupdate) - this->systemsPostupdate.push_back(system.postupdate); + if (_system.postupdate) + this->systemsPostupdate.push_back(_system.postupdate); } ///////////////////////////////////////////////// @@ -473,7 +506,6 @@ void SimulationRunner::ProcessSystemQueue() { this->AddSystemToRunner(system); } - this->pendingSystems.clear(); // If additional systems were added, recreate the worker threads. @@ -483,9 +515,9 @@ void SimulationRunner::ProcessSystemQueue() << this->systemsPostupdate.size() + 1 << std::endl; this->postUpdateStartBarrier = - std::make_unique(this->systemsPostupdate.size() + 1); + std::make_unique(this->systemsPostupdate.size() + 1u); this->postUpdateStopBarrier = - std::make_unique(this->systemsPostupdate.size() + 1); + std::make_unique(this->systemsPostupdate.size() + 1u); this->postUpdateThreadsRunning = true; int id = 0; @@ -827,18 +859,10 @@ void SimulationRunner::LoadPlugin(const Entity _entity, system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); } - // System correctly loaded from library, try to configure + // System correctly loaded from library if (system) { - auto systemConfig = system.value()->QueryInterface(); - if (systemConfig != nullptr) - { - systemConfig->Configure(_entity, _sdf, - this->entityCompMgr, - this->eventMgr); - } - - this->AddSystem(system.value()); + this->AddSystem(system.value(), _entity, _sdf); igndbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index e9d0316f63..4362044566 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -88,26 +89,50 @@ namespace ignition std::chrono::steady_clock::duration seek{-1}; }; - /// \brief Class to hold systems internally + /// \brief Class to hold systems internally. It supports systems loaded + /// from plugins, as well as systems created at runtime. class SystemInternal { /// \brief Constructor + /// \param[in] _systemPlugin A system loaded from a plugin. public: explicit SystemInternal(SystemPluginPtr _systemPlugin) : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), + configure(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), postupdate(systemPlugin->QueryInterface()) { } + /// \brief Constructor + /// \param[in] _system Pointer to a system. + public: explicit SystemInternal(const std::shared_ptr &_system) + : systemShared(_system), + system(_system.get()), + configure(dynamic_cast(_system.get())), + preupdate(dynamic_cast(_system.get())), + update(dynamic_cast(_system.get())), + postupdate(dynamic_cast(_system.get())) + { + } + /// \brief Plugin object. This manages the lifecycle of the instantiated /// class as well as the shared library. + /// This will be null if the system wasn't loaded from a plugin. public: SystemPluginPtr systemPlugin; + /// \brief Pointer to a system. + /// This will be null if the system wasn't loaded from a pointer. + public: std::shared_ptr systemShared{nullptr}; + /// \brief Access this system via the `System` interface public: System *system = nullptr; + /// \brief Access this system via the ISystemConfigure interface + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemConfigure *configure = nullptr; + /// \brief Access this system via the ISystemPreUpdate interface /// Will be nullptr if the System doesn't implement this interface. public: ISystemPreUpdate *preupdate = nullptr; @@ -160,9 +185,31 @@ namespace ignition /// \brief Add system after the simulation runner has been instantiated /// \note This actually adds system to a queue. The system is added to the - /// runner at the begining of the a simulation cycle (call to Run) + /// runner at the begining of the a simulation cycle (call to Run). It is + /// also responsible for calling `Configure` on the system. + /// \param[in] _system SystemPluginPtr to be added + /// \param[in] _entity Entity that system is attached to. If nullopt, + /// system is attached to a world. + /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to + /// SDF of the entire world. + public: void AddSystem(const SystemPluginPtr &_system, + std::optional _entity = std::nullopt, + std::optional> _sdf = + std::nullopt); + + /// \brief Add system after the simulation runner has been instantiated + /// \note This actually adds system to a queue. The system is added to the + /// runner at the begining of the a simulation cycle (call to Run). It is + /// also responsible for calling `Configure` on the system. /// \param[in] _system System to be added - public: void AddSystem(const SystemPluginPtr &_system); + /// \param[in] _entity Entity of system to be added. Nullopt if system + /// doesn't connect to an entity. + /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to + /// world. + public: void AddSystem(const std::shared_ptr &_system, + std::optional _entity = std::nullopt, + std::optional> _sdf = + std::nullopt); /// \brief Update all the systems public: void UpdateSystems(); @@ -339,7 +386,7 @@ namespace ignition /// \brief Actually add system to the runner /// \param[in] _system System to be added - public: void AddSystemToRunner(const SystemPluginPtr &_system); + public: void AddSystemToRunner(SystemInternal _system); /// \brief Calls AddSystemToRunner to each system that is pending to be /// added. @@ -376,6 +423,16 @@ namespace ignition /// Physics component of the world, if any. public: void UpdatePhysicsParams(); + /// \brief Implementation for AddSystem functions. This only adds systems + /// to a queue, the actual addition is performed by `AddSystemToRunner` at + /// the appropriate time. + /// \param[in] _system Generic representation of a system. + /// \param[in] _entity Entity received from AddSystem. + /// \param[in] _sdf SDF received from AddSystem. + private: void AddSystemImpl(SystemInternal _system, + std::optional _entity = std::nullopt, + std::optional> _sdf = std::nullopt); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -387,7 +444,7 @@ namespace ignition private: std::vector systems; /// \brief Pending systems to be added to systems. - private: std::vector pendingSystems; + private: std::vector pendingSystems; /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; diff --git a/src/Util.cc b/src/Util.cc index d6af1281ea..cd3cf9a01f 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -61,8 +61,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { math::Pose3d worldPose(const Entity &_entity, const EntityComponentManager &_ecm) { + auto poseComp = _ecm.Component(_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(_entity)->Data(); + math::Pose3d pose = poseComp->Data(); auto p = _ecm.Component(_entity); while (p) { @@ -127,6 +135,64 @@ std::string scopedName(const Entity &_entity, return result; } +////////////////////////////////////////////////// +std::unordered_set 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 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 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 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(resVector.begin(), resVector.end()); +} + ////////////////////////////////////////////////// ComponentTypeId entityTypeId(const Entity &_entity, const EntityComponentManager &_ecm) @@ -246,6 +312,12 @@ Entity worldEntity(const Entity &_entity, return entity; } +////////////////////////////////////////////////// +Entity worldEntity(const EntityComponentManager &_ecm) +{ + return _ecm.EntityByComponents(components::World()); +} + ////////////////////////////////////////////////// std::string removeParentScope(const std::string &_name, const std::string &_delim) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 15e12002c9..ee2d410d19 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -70,6 +70,7 @@ TEST_F(UtilTest, ScopedName) // World auto worldEntity = ecm.CreateEntity(); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(ecm)); EXPECT_EQ(kNullEntity, gazebo::worldEntity(worldEntity, ecm)); ecm.CreateComponent(worldEntity, components::World()); ecm.CreateComponent(worldEntity, components::Name("world_name")); @@ -213,6 +214,7 @@ TEST_F(UtilTest, ScopedName) "world_name::actorD_name"); // World entity + EXPECT_EQ(worldEntity, gazebo::worldEntity(ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(worldEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(lightAEntity, ecm)); EXPECT_EQ(worldEntity, gazebo::worldEntity(modelBEntity, ecm)); @@ -230,6 +232,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 &_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) { @@ -609,3 +693,28 @@ TEST_F(UtilTest, TopicFromScopedName) EXPECT_TRUE(topicFromScopedName(worldEntity, ecm).empty()); EXPECT_EQ(worldName, topicFromScopedName(worldEntity, ecm, false)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, EnableComponent) +{ + EntityComponentManager ecm; + + auto entity1 = ecm.CreateEntity(); + EXPECT_EQ(nullptr, ecm.Component(entity1)); + + // Enable + EXPECT_TRUE(enableComponent(ecm, entity1)); + EXPECT_NE(nullptr, ecm.Component(entity1)); + + // Enabling again makes no changes + EXPECT_FALSE(enableComponent(ecm, entity1, true)); + EXPECT_NE(nullptr, ecm.Component(entity1)); + + // Disable + EXPECT_TRUE(enableComponent(ecm, entity1, false)); + EXPECT_EQ(nullptr, ecm.Component(entity1)); + + // Disabling again makes no changes + EXPECT_FALSE(enableComponent(ecm, entity1, false)); + EXPECT_EQ(nullptr, ecm.Component(entity1)); +} diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 9b9209d407..37595e65db 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -176,8 +176,6 @@ std::unique_ptr createGui( // which makes it complicated to mix configurations across worlds. // We could have a way to use world-agnostic topics like Gazebo-classic's ~ auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0)); - runner->connect(app.get(), &ignition::gui::Application::PluginAdded, runner, - &ignition::gazebo::GuiRunner::OnPluginAdded); ++runnerCount; runner->setParent(ignition::gui::App()); @@ -227,8 +225,6 @@ std::unique_ptr createGui( // GUI runner auto runner = new ignition::gazebo::GuiRunner(worldName); - runner->connect(app.get(), &ignition::gui::Application::PluginAdded, - runner, &ignition::gazebo::GuiRunner::OnPluginAdded); runner->setParent(ignition::gui::App()); ++runnerCount; diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 4e3ef1b4c7..b44ae6219f 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -128,25 +128,10 @@ void GuiRunner::RequestState() } ///////////////////////////////////////////////// -void GuiRunner::OnPluginAdded(const QString &_objectName) +void GuiRunner::OnPluginAdded(const QString &) { - auto plugin = gui::App()->PluginByName(_objectName.toStdString()); - if (!plugin) - { - ignerr << "Failed to get plugin [" << _objectName.toStdString() - << "]" << std::endl; - return; - } - - this->RequestState(); - - auto guiSystem = dynamic_cast(plugin.get()); - - // Do nothing for pure ign-gui plugins - if (!guiSystem) - return; - - guiSystem->Update(this->updateInfo, this->ecm); + // This function used to call Update on the plugin, but that's no longer + // necessary. The function is left here for ABI compatibility. } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index c37e679e72..fdcdaf90a4 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -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(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) diff --git a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc index 2c40082982..703fa7de84 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc @@ -68,8 +68,6 @@ TEST_F(JointPositionControllerGui, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) // Create GUI runner to handle gazebo::gui plugins auto runner = new gazebo::GuiRunner("test"); - runner->connect(app.get(), &gui::Application::PluginAdded, - runner, &gazebo::GuiRunner::OnPluginAdded); runner->setParent(gui::App()); // Add plugin @@ -85,6 +83,17 @@ TEST_F(JointPositionControllerGui, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) EXPECT_EQ(plugins.size(), 1); auto plugin = plugins[0]; + + int sleep = 0; + int maxSleep = 30; + while (plugin->ModelName() != "No model selected" && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_LT(sleep, maxSleep); EXPECT_EQ(plugin->Title(), "Joint position controller"); EXPECT_EQ(plugin->ModelEntity(), gazebo::kNullEntity); EXPECT_EQ(plugin->ModelName(), QString("No model selected")) @@ -143,8 +152,6 @@ TEST_F(JointPositionControllerGui, // Create GUI runner to handle gazebo::gui plugins auto runner = new gazebo::GuiRunner("test"); - runner->connect(app.get(), &gui::Application::PluginAdded, - runner, &gazebo::GuiRunner::OnPluginAdded); runner->setParent(gui::App()); // Load plugin @@ -175,6 +182,16 @@ TEST_F(JointPositionControllerGui, auto plugin = plugins[0]; EXPECT_EQ(plugin->Title(), "JointPositionController!"); + int sleep = 0; + int maxSleep = 30; + while (plugin->ModelName() != "No model selected" && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + EXPECT_EQ(plugin->ModelEntity(), gazebo::kNullEntity); EXPECT_EQ(plugin->ModelName(), QString("No model selected")) << plugin->ModelName().toStdString(); @@ -196,8 +213,7 @@ TEST_F(JointPositionControllerGui, runner->RequestState(); }); - int sleep = 0; - int maxSleep = 30; + sleep = 0; while (plugin->ModelName() != "model_name" && sleep < maxSleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -217,4 +233,3 @@ TEST_F(JointPositionControllerGui, // Cleanup plugins.clear(); } - diff --git a/src/gui/plugins/plot_3d/CMakeLists.txt b/src/gui/plugins/plot_3d/CMakeLists.txt new file mode 100644 index 0000000000..3ff591a861 --- /dev/null +++ b/src/gui/plugins/plot_3d/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(Plot3D + SOURCES + Plot3D.cc + QT_HEADERS + Plot3D.hh + TEST_SOURCES + Plot3D_TEST.cc +) diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc new file mode 100644 index 0000000000..45565bbc75 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -0,0 +1,405 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/Util.hh" + +#include "Plot3D.hh" + +namespace ignition::gazebo::gui +{ + /// \brief Private data class for Plot3D + class Plot3DPrivate + { + /// \brief Transport node. + public: transport::Node node; + + /// \brief Whether currently locked on a given entity + public: bool locked{false}; + + /// \brief Current target entity. + public: Entity targetEntity{kNullEntity}; + + /// \brief Name of the target entity. + public: std::string targetName; + + /// \brief Whether the target entity has been changed. + public: bool targetEntityDirty{false}; + + /// \brief Whether the target name has been changed. + public: bool targetNameDirty{false}; + + /// \brief Current marker message. + public: msgs::Marker markerMsg; + + /// \brief Marker color. + public: math::Color color{math::Color::Blue}; + + /// \brief Previous plotted position. + public: math::Vector3d prevPos; + + /// \brief Offset from entity origin to place plot point. + /// The offset is expressed in the entity's frame. + public: math::Vector3d offset; + + /// \brief Minimum distance between points. If the object has moved by less + /// than this distance, a new point isn't plotted. + public: double minDistance{0.05}; + + /// \brief Maximum number of points in the plot. When the plot reaches this + /// size, older points are deleted. + public: int maxPoints{1000}; + + /// \brief Protects variables that are updated by the user. + public: std::mutex mutex; + }; +} + +using namespace ignition; +using namespace ignition::gazebo; +using namespace ignition::gazebo::gui; + +///////////////////////////////////////////////// +Plot3D::Plot3D() + : GuiSystem(), dataPtr(std::make_unique()) +{ + qRegisterMetaType("Entity"); +} + +///////////////////////////////////////////////// +Plot3D::~Plot3D() +{ + this->ClearPlot(); +} + +///////////////////////////////////////////////// +void Plot3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "3D Plot"; + + // Parameters from SDF + if (_pluginElem) + { + auto nameElem = _pluginElem->FirstChildElement("entity_name"); + if (nullptr != nameElem && nullptr != nameElem->GetText()) + { + this->dataPtr->targetName = nameElem->GetText(); + this->dataPtr->targetNameDirty = true; + this->SetLocked(true); + } + + auto offsetElem = _pluginElem->FirstChildElement("offset"); + if (nullptr != offsetElem && nullptr != offsetElem->GetText()) + { + std::stringstream offsetStr; + offsetStr << std::string(offsetElem->GetText()); + offsetStr >> this->dataPtr->offset; + this->OffsetChanged(); + } + + auto colorElem = _pluginElem->FirstChildElement("color"); + if (nullptr != colorElem && nullptr != colorElem->GetText()) + { + std::stringstream colorStr; + colorStr << std::string(colorElem->GetText()); + colorStr >> this->dataPtr->color; + this->ColorChanged(); + } + + auto distElem = _pluginElem->FirstChildElement("minimum_distance"); + if (nullptr != distElem && nullptr != distElem->GetText()) + { + distElem->QueryDoubleText(&this->dataPtr->minDistance); + this->MinDistanceChanged(); + } + + auto ptsElem = _pluginElem->FirstChildElement("maximum_points"); + if (nullptr != ptsElem && nullptr != ptsElem->GetText()) + { + ptsElem->QueryIntText(&this->dataPtr->maxPoints); + this->MaxPointsChanged(); + } + } + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void Plot3D::ClearPlot() +{ + // Clear previous plot + if (this->dataPtr->markerMsg.point().size() > 0) + { + this->dataPtr->markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); + } +} + +////////////////////////////////////////////////// +void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) +{ + std::lock_guard lock(this->dataPtr->mutex); + bool newTarget{false}; + + // New target by name, get entity + if (this->dataPtr->targetNameDirty) + { + auto entities = entitiesFromScopedName(this->dataPtr->targetName, _ecm); + if (entities.empty()) + { + // Keep trying + return; + } + + Entity entity = *(entities.begin()); + + if (kNullEntity == entity) + { + // Keep trying + return; + } + + this->dataPtr->targetEntity = entity; + this->dataPtr->targetNameDirty = false; + newTarget = true; + } + + // New target by entity, get name + if (this->dataPtr->targetEntityDirty) + { + this->dataPtr->targetEntityDirty = false; + + auto name = _ecm.ComponentData( + this->dataPtr->targetEntity); + if (!name) + { + this->dataPtr->targetName.clear(); + return; + } + this->dataPtr->targetName = name.value(); + + newTarget = true; + } + + if (newTarget) + { + this->ClearPlot(); + + // Reset message + this->dataPtr->markerMsg.Clear(); + this->dataPtr->markerMsg.set_ns("plot_" + this->dataPtr->targetName); + this->dataPtr->markerMsg.set_id(this->dataPtr->targetEntity); + this->dataPtr->markerMsg.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->markerMsg.set_type(msgs::Marker::LINE_STRIP); + this->dataPtr->markerMsg.set_visibility(msgs::Marker::GUI); + + // Update view + this->TargetEntityChanged(); + this->TargetNameChanged(); + } + + // Get entity pose + auto pose = worldPose(this->dataPtr->targetEntity, _ecm); + math::Pose3d offsetPose; + offsetPose.Set(this->dataPtr->offset, math::Vector3d::Zero); + + auto point = (pose * offsetPose).Pos(); + + // Only add points if the distance is past a threshold. + if (point.Distance(this->dataPtr->prevPos) < this->dataPtr->minDistance) + return; + + this->dataPtr->prevPos = point; + ignition::msgs::Set(this->dataPtr->markerMsg.add_point(), point); + + // Reduce message array + if (this->dataPtr->markerMsg.point_size() > this->dataPtr->maxPoints) + this->dataPtr->markerMsg.mutable_point()->DeleteSubrange(0, 5); + + msgs::Set(this->dataPtr->markerMsg.mutable_material()->mutable_ambient(), + this->dataPtr->color); + msgs::Set(this->dataPtr->markerMsg.mutable_material()->mutable_diffuse(), + this->dataPtr->color); + + // Request + this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); +} + +///////////////////////////////////////////////// +bool Plot3D::eventFilter(QObject *_obj, QEvent *_event) +{ + if (!this->dataPtr->locked) + { + if (_event->type() == gazebo::gui::events::EntitiesSelected::kType) + { + auto event = reinterpret_cast(_event); + if (event && !event->Data().empty()) + { + this->SetTargetEntity(*event->Data().begin()); + } + } + + if (_event->type() == gazebo::gui::events::DeselectAllEntities::kType) + { + auto event = reinterpret_cast( + _event); + if (event) + { + this->SetTargetEntity(kNullEntity); + } + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +Entity Plot3D::TargetEntity() const +{ + return this->dataPtr->targetEntity; +} + +///////////////////////////////////////////////// +void Plot3D::SetTargetEntity(Entity _entity) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->targetEntity = _entity; + this->dataPtr->targetEntityDirty = true; + this->TargetEntityChanged(); + + if (this->dataPtr->targetEntity == kNullEntity) + { + this->dataPtr->targetName.clear(); + } +} + +///////////////////////////////////////////////// +QString Plot3D::TargetName() const +{ + return QString::fromStdString(this->dataPtr->targetName); +} + +///////////////////////////////////////////////// +void Plot3D::SetTargetName(const QString &_targetName) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->targetName = _targetName.toStdString(); + this->dataPtr->targetNameDirty = true; + this->TargetNameChanged(); +} + +///////////////////////////////////////////////// +bool Plot3D::Locked() const +{ + return this->dataPtr->locked; +} + +///////////////////////////////////////////////// +void Plot3D::SetLocked(bool _locked) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->locked = _locked; + this->LockedChanged(); +} + +///////////////////////////////////////////////// +QVector3D Plot3D::Offset() const +{ + return QVector3D( + this->dataPtr->offset.X(), + this->dataPtr->offset.Y(), + this->dataPtr->offset.Z()); +} + +///////////////////////////////////////////////// +void Plot3D::SetOffset(const QVector3D &_offset) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->offset.Set(_offset.x(), _offset.y(), _offset.z()); + this->OffsetChanged(); +} + +///////////////////////////////////////////////// +QVector3D Plot3D::Color() const +{ + return QVector3D( + this->dataPtr->color.R(), + this->dataPtr->color.G(), + this->dataPtr->color.B()); +} + +///////////////////////////////////////////////// +void Plot3D::SetColor(const QVector3D &_color) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->color.Set(_color.x(), _color.y(), _color.z()); + this->ColorChanged(); +} + +///////////////////////////////////////////////// +double Plot3D::MinDistance() const +{ + return this->dataPtr->minDistance; +} + +///////////////////////////////////////////////// +void Plot3D::SetMinDistance(double _minDistance) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->minDistance = _minDistance; + this->MinDistanceChanged(); +} + +///////////////////////////////////////////////// +int Plot3D::MaxPoints() const +{ + return this->dataPtr->maxPoints; +} + +///////////////////////////////////////////////// +void Plot3D::SetMaxPoints(int _maxPoints) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->maxPoints = _maxPoints; + this->MaxPointsChanged(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::gui::Plot3D, + ignition::gui::Plugin) diff --git a/src/gui/plugins/plot_3d/Plot3D.hh b/src/gui/plugins/plot_3d/Plot3D.hh new file mode 100644 index 0000000000..faf4466024 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.hh @@ -0,0 +1,228 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_3DPLOT_HH_ +#define IGNITION_GAZEBO_GUI_3DPLOT_HH_ + +#include + +#include + +#include "ignition/gui/qt.h" + +namespace ignition +{ +namespace gazebo +{ +namespace gui +{ + class Plot3DPrivate; + + /// \brief Plot the trajectory of an entity into the 3D scene. + /// + /// This plugin can be instantiated multiple times to plot various entities. + /// + /// The plugin is automatically attached to the currently selected entity, + /// unless it is locked on an entity. + /// + /// ## Configuration + /// + /// * `` (optional): Plot the given entity at startup. Accepts + /// names scoped with `::`, for example `my_model::my_link`. If not provided, + /// the plugin starts not attached to any entity, and will attach to the + /// next selected entity. + /// + /// * `` (optional): RGB color of line, defaults to blue. + /// + /// * `` (optional): XYZ offset from the entity's origin to plot from, + /// expressed in the entity's frame. Defaults to zero. + /// + /// * `` (optional): The minimum distance between points to + /// plot. A new point will not be plotted until the entity has moved beyond + /// this distance from the previous point. Defaults to 0.05 m. + /// + /// * ` (optional)`: Maximum number of points on the plot. + /// After this number is reached, the older points start being deleted. + /// Defaults to 1000. + /// + class Plot3D : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Target entity + Q_PROPERTY( + Entity targetEntity + READ TargetEntity + WRITE SetTargetEntity + NOTIFY TargetEntityChanged + ) + + /// \brief Target entity scoped name + Q_PROPERTY( + QString targetName + READ TargetName + WRITE SetTargetName + NOTIFY TargetNameChanged + ) + + /// \brief Whether the plugin is locked on an entity + Q_PROPERTY( + bool locked + READ Locked + WRITE SetLocked + NOTIFY LockedChanged + ) + + /// \brief XYZ offset to the entity's origin to plot + Q_PROPERTY( + QVector3D offset + READ Offset + WRITE SetOffset + NOTIFY OffsetChanged + ) + + /// \brief Plot line color + Q_PROPERTY( + QVector3D color + READ Color + WRITE SetColor + NOTIFY ColorChanged + ) + + /// \brief Minimum distance between plot points + Q_PROPERTY( + double minDistance + READ MinDistance + WRITE SetMinDistance + NOTIFY MinDistanceChanged + ) + + /// \brief Maximum number of total points on the plot + Q_PROPERTY( + int maxPoints + READ MaxPoints + WRITE SetMaxPoints + NOTIFY MaxPointsChanged + ) + + /// \brief Constructor + public: Plot3D(); + + /// \brief Destructor + public: ~Plot3D() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Get the target currently controlled. + /// \return Target entity ID. + public: Q_INVOKABLE Entity TargetEntity() const; + + /// \brief Set the target currently controlled. + /// \param[in] _entity Target entity ID. + public: Q_INVOKABLE void SetTargetEntity(Entity _entity); + + /// \brief Notify that entity has changed. + signals: void TargetEntityChanged(); + + /// \brief Get the name of target currently controlled. + /// \return TargetName, such as 'world' or 'target' + public: Q_INVOKABLE QString TargetName() const; + + /// \brief Set the name of target currently controlled. + /// \param[in] _name TargetName, such as 'world' or 'target'. + public: Q_INVOKABLE void SetTargetName(const QString &_name); + + /// \brief Notify that target name has changed + signals: void TargetNameChanged(); + + /// \brief Get whether the plugin is currently locked on a target. + /// \return True for locked + public: Q_INVOKABLE bool Locked() const; + + /// \brief Set whether the plugin is currently locked on a target. + /// \param[in] _locked True for locked. + public: Q_INVOKABLE void SetLocked(bool _locked); + + /// \brief Notify that locked has changed. + signals: void LockedChanged(); + + /// \brief Get the offset in the target's frame. + /// \return The current offset. + public: Q_INVOKABLE QVector3D Offset() const; + + /// \brief Set the offset. + /// \param[in] _offset The offset in the target's frame + public: Q_INVOKABLE void SetOffset(const QVector3D &_offset); + + /// \brief Notify that the offset has changed. + signals: void OffsetChanged(); + + /// \brief Get the color of the plot. + /// \return The current color. + public: Q_INVOKABLE QVector3D Color() const; + + /// \brief Set the color of the plot. + /// \param[in] _color New color. + public: Q_INVOKABLE void SetColor(const QVector3D &_color); + + /// \brief Notify that the color has changed. + signals: void ColorChanged(); + + /// \brief Get the minimum distance between points. + /// \return The current minimum distance. + public: Q_INVOKABLE double MinDistance() const; + + /// \brief Set the minimum distance between points. If the target moved + /// less than this distance, the latest position won't be plotted. + /// \param[in] _minDistance New distance. + public: Q_INVOKABLE void SetMinDistance(double _minDistance); + + /// \brief Notify that the minimum distance has changed. + signals: void MinDistanceChanged(); + + /// \brief Get the maximum number of points. + /// \return The current maximum points. + public: Q_INVOKABLE int MaxPoints() const; + + /// \brief Set the maximum number of points. If the plot has more than + /// this number, older points start being removed. + /// \param[in] _maxPoints Maximum number of points. + public: Q_INVOKABLE void SetMaxPoints(int _maxPoints); + + /// \brief Notify that the maximum points has changed. + signals: void MaxPointsChanged(); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Clear plot + private: void ClearPlot(); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/plot_3d/Plot3D.qml b/src/gui/plugins/plot_3d/Plot3D.qml new file mode 100644 index 0000000000..1fbc9e2f6a --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.qml @@ -0,0 +1,317 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/qml" + +Rectangle { + id: plot3D + color: "transparent" + Layout.minimumWidth: 400 + Layout.minimumHeight: 320 + anchors.fill: parent + + /** + * Light grey according to theme + */ + property color lightGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade100) : + Material.color(Material.Grey, Material.Shade800) + + /** + * Dark grey according to theme + */ + property color darkGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade200) : + Material.color(Material.Grey, Material.Shade900) + + Connections { + target: Plot3D + onLockedChanged: { + lockButton.checked = Plot3D.Locked() + } + } + + Rectangle { + id: header + height: lockButton.height + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + width: parent.width + color: darkGrey + + RowLayout { + anchors.fill: parent + spacing: 0 + + Label { + text: Plot3D.targetName.empty ? "No entity selected" : Plot3D.targetName + color: Material.theme == Material.Light ? "#444444" : "#cccccc" + font.pointSize: 12 + padding: 3 + } + + Item { + height: entityLabel.height + Layout.fillWidth: true + } + + ToolButton { + id: lockButton + checkable: true + checked: false + text: "Lock entity" + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: lockButton.checked ? "qrc:/Gazebo/images/lock.svg" : + "qrc:/Gazebo/images/unlock.svg" + sourceSize.width: 18; + sourceSize.height: 18; + } + ToolTip.text: lockButton.checked ? "Unlock target selection" + : "Lock target selection" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onToggled: { + Plot3D.locked = lockButton.checked + } + } + + Label { + id: entityLabel + text: 'Entity ' + Plot3D.targetEntity + Layout.minimumWidth: 80 + color: Material.theme == Material.Light ? "#444444" : "#cccccc" + font.pointSize: 12 + padding: 5 + } + } + } + ColumnLayout { + anchors.top: header.bottom + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right + + GridLayout { + Layout.fillWidth: true + columns: 6 + + Text { + text: "Offset" + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 + } + + Text { + text: "X (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: x + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: Plot3D.offset.x + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } + Text { + text: "Y (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: y + Layout.fillWidth: true + value: Plot3D.offset.y + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 4 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 5 + value: Plot3D.offset.z + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetOffset(Qt.vector3d(x.value, y.value, z.value)) + } + + Text { + text: "Color" + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 0 + Layout.columnSpan: 6 + color: "dimgrey" + font.bold: true + leftPadding: 5 + } + + Text { + text: "R" + color: "dimgrey" + Layout.row: 3 + Layout.column: 0 + leftPadding: 5 + } + + IgnSpinBox { + id: r + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 1 + value: Plot3D.color.x + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } + + Text { + text: "G" + Layout.row: 3 + Layout.column: 2 + color: "dimgrey" + leftPadding: 5 + } + + IgnSpinBox { + id: g + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 3 + value: Plot3D.color.y + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } + + Text { + text: "B" + Layout.row: 3 + Layout.column: 4 + color: "dimgrey" + leftPadding: 5 + } + + IgnSpinBox { + id: b + Layout.fillWidth: true + Layout.row: 3 + Layout.column: 5 + value: Plot3D.color.z + maximumValue: 1.00 + minimumValue: 0.00 + decimals: 2 + stepSize: 0.01 + onEditingFinished: Plot3D.SetColor(Qt.vector3d(r.value, g.value, b.value)) + } + } + + GridLayout { + Layout.fillWidth: true + columns: 2 + + Text { + text: "Min distance between points (m)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: minDist + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: Plot3D.minDistance + maximumValue: 1000000 + minimumValue: 0 + decimals: 6 + stepSize: 0.01 + onEditingFinished: Plot3D.SetMinDistance(minDist.value) + } + + Text { + text: "Max points" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: maxPoints + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: Plot3D.maxPoints + maximumValue: 1000000 + minimumValue: 0 + decimals: 0 + stepSize: 100 + onEditingFinished: Plot3D.SetMaxPoints(maxPoints.value) + } + } + + // Bottom spacer + Item { + width: 10 + Layout.row: 6 + Layout.column: 0 + Layout.columnSpan: 6 + Layout.fillHeight: true + } + } +} diff --git a/src/gui/plugins/plot_3d/Plot3D.qrc b/src/gui/plugins/plot_3d/Plot3D.qrc new file mode 100644 index 0000000000..f1327f014d --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D.qrc @@ -0,0 +1,5 @@ + + + Plot3D.qml + + diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc new file mode 100644 index 0000000000..f34e600409 --- /dev/null +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/test_config.hh" + +// Use this when forward-porting to v6 +// #include "../../GuiRunner.hh" +#include "ignition/gazebo/gui/GuiRunner.hh" + +#include "Plot3D.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("dummy")), +}; + +using namespace ignition; + +/// \brief Tests for the joint position controller GUI plugin +class Plot3D : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(Plot3D, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) +{ + // Create app + auto app = std::make_unique(g_argc, g_argv); + ASSERT_NE(nullptr, app); + app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Create GUI runner to handle gazebo::gui plugins + auto runner = new gazebo::GuiRunner("test"); + runner->setParent(gui::App()); + + // Add plugin + const char *pluginStr = + "" + "" + "Plot3D!" + "" + "banana" + "123" + "0.123" + "1 2 3" + "0.1 0.2 0.3" + ""; + + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + EXPECT_TRUE(app->LoadPlugin("Plot3D", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app->findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + ASSERT_EQ(plugins.size(), 1); + + auto plugin = plugins[0]; + EXPECT_EQ("Plot3D!", plugin->Title()); + EXPECT_EQ(gazebo::kNullEntity, plugin->TargetEntity()); + EXPECT_EQ(QString("banana"), plugin->TargetName()) + << plugin->TargetName().toStdString(); + EXPECT_EQ(QVector3D(0.1, 0.2, 0.3), plugin->Color()); + EXPECT_EQ(QVector3D(1, 2, 3), plugin->Offset()); + EXPECT_TRUE(plugin->Locked()); + + // Cleanup + plugins.clear(); +} diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 362793fc8c..3a7018bc58 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1398,9 +1398,6 @@ void IgnRenderer::HandleMouseTransformControl() // Select entity else if (!this->dataPtr->mouseEvent.Dragging()) { - rendering::VisualPtr v = this->dataPtr->camera->VisualAt( - this->dataPtr->mouseEvent.Pos()); - rendering::VisualPtr visual = this->dataPtr->camera->Scene()->VisualAt( this->dataPtr->camera, this->dataPtr->mouseEvent.Pos()); diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 3726eb69e9..00978cc26f 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -128,40 +128,11 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, transport::Node node; this->dataPtr->pub = node.Advertise(topic); - if (!_ecm.Component(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldPose()); - } + Link link(this->dataPtr->linkEntity); + link.EnableVelocityChecks(_ecm, true); - if (!_ecm.Component(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, components::Inertial()); - } - - // Create a world linear velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldLinearVelocity()); - } - - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::AngularVelocity()); - } - - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldAngularVelocity()); - } + // Create a default inertia in case the link doesn't have it + enableComponent(_ecm, this->dataPtr->linkEntity, true); } ////////////////////////////////////////////////// diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 1d1194bc7e..a9e63d1764 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -498,24 +498,8 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) if (this->dataPtr->validConfig) { - if (!_ecm.Component(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldPose()); - } - - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldAngularVelocity()); - } + Link link(this->dataPtr->linkEntity); + link.EnableVelocityChecks(_ecm, true); if ((this->dataPtr->controlJointEntity != kNullEntity) && !_ecm.Component( diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index d1a2e310dd..5389ba9c17 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -555,17 +555,8 @@ void WindEffects::PreUpdate(const UpdateInfo &_info, { if (_windMode->Data()) { - // Create a WorldLinearVelocity component on the link so that - // physics can populate it - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, components::WorldPose()); - } + Link link(_entity); + link.EnableVelocityChecks(_ecm, true); } return true; }); diff --git a/test/helpers/Relay.hh b/test/helpers/Relay.hh index e761d6eff6..7ff35e2cd8 100644 --- a/test/helpers/Relay.hh +++ b/test/helpers/Relay.hh @@ -19,7 +19,6 @@ #include -#include #include #include "../plugins/MockSystem.hh" @@ -54,22 +53,15 @@ class Relay /// \brief Constructor public: Relay() { - auto plugin = loader.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", nullptr); - - EXPECT_TRUE(plugin.has_value()); - this->systemPtr = plugin.value(); - - this->mockSystem = static_cast( - systemPtr->QueryInterface()); - EXPECT_NE(nullptr, this->mockSystem); + this->systemPtr = std::make_shared(); + EXPECT_NE(nullptr, this->systemPtr); } /// \brief Wrapper around system's pre-update callback /// \param[in] _cb Function to be called every pre-update public: Relay &OnPreUpdate(MockSystem::CallbackType _cb) { - this->mockSystem->preUpdateCallback = std::move(_cb); + this->systemPtr->preUpdateCallback = std::move(_cb); return *this; } @@ -77,7 +69,7 @@ class Relay /// \param[in] _cb Function to be called every update public: Relay &OnUpdate(MockSystem::CallbackType _cb) { - this->mockSystem->updateCallback = std::move(_cb); + this->systemPtr->updateCallback = std::move(_cb); return *this; } @@ -85,18 +77,12 @@ class Relay /// \param[in] _cb Function to be called every post-update public: Relay &OnPostUpdate(MockSystem::CallbackTypeConst _cb) { - this->mockSystem->postUpdateCallback = std::move(_cb); + this->systemPtr->postUpdateCallback = std::move(_cb); return *this; } - /// \brief Pointer to underlying syste, - public: SystemPluginPtr systemPtr; - - /// \brief Pointer to underlying mock interface - public: MockSystem *mockSystem; - - /// \brief Used to load the system. - private: SystemLoader loader; + /// \brief Pointer to system + public: std::shared_ptr systemPtr{nullptr}; }; } } diff --git a/test/integration/level_manager.cc b/test/integration/level_manager.cc index d9e8858e26..e61da0ed5a 100644 --- a/test/integration/level_manager.cc +++ b/test/integration/level_manager.cc @@ -60,7 +60,7 @@ class ModelMover: public test::Relay public: explicit ModelMover(Entity _entity): test::Relay(), entity(_entity) { using namespace std::placeholders; - this->mockSystem->preUpdateCallback = + this->systemPtr->preUpdateCallback = std::bind(&ModelMover::MoveModel, this, _1, _2); } diff --git a/test/integration/level_manager_runtime_performers.cc b/test/integration/level_manager_runtime_performers.cc index ccb8b63ba8..b873719afc 100644 --- a/test/integration/level_manager_runtime_performers.cc +++ b/test/integration/level_manager_runtime_performers.cc @@ -57,7 +57,7 @@ class ModelMover: public test::Relay public: explicit ModelMover(Entity _entity): test::Relay(), entity(_entity) { using namespace std::placeholders; - this->mockSystem->preUpdateCallback = + this->systemPtr->preUpdateCallback = std::bind(&ModelMover::MoveModel, this, _1, _2); } diff --git a/test/integration/link.cc b/test/integration/link.cc index bb112a6448..4b69e71c0c 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -256,17 +256,28 @@ TEST_F(LinkIntegrationTest, LinkVelocities) ASSERT_TRUE(link.Valid(ecm)); - // Before we add components, velocity functions should return nullopt + // Before enabling, velocity functions should return nullopt EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm)); EXPECT_EQ(std::nullopt, link.WorldAngularVelocity(ecm)); + // After enabling, velocity functions should return default values + link.EnableVelocityChecks(ecm); + + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_NE(nullptr, ecm.Component(eLink)); + + EXPECT_EQ(math::Vector3d::Zero, link.WorldLinearVelocity(ecm)); + EXPECT_EQ(math::Vector3d::Zero, link.WorldAngularVelocity(ecm)); + + // With custom velocities math::Pose3d pose; pose.Set(0, 0, 0, IGN_PI_2, 0, 0); math::Vector3d linVel{1.0, 0.0, 0.0}; math::Vector3d angVel{0.0, 0.0, 2.0}; - ecm.CreateComponent(eLink, components::WorldPose(pose)); - ecm.CreateComponent(eLink, components::WorldLinearVelocity(linVel)); - ecm.CreateComponent(eLink, components::WorldAngularVelocity(angVel)); + ecm.SetComponentData(eLink, pose); + ecm.SetComponentData(eLink, linVel); + ecm.SetComponentData(eLink, angVel); EXPECT_EQ(linVel, link.WorldLinearVelocity(ecm)); EXPECT_EQ(angVel, link.WorldAngularVelocity(ecm)); @@ -276,6 +287,16 @@ TEST_F(LinkIntegrationTest, LinkVelocities) math::Vector3d angVelBody = pose.Rot().RotateVectorReverse(angVel); auto expLinVel = linVel + pose.Rot().RotateVector(angVelBody.Cross(offset)); EXPECT_EQ(expLinVel, link.WorldLinearVelocity(ecm, offset)); + + // Disabling velocities goes back to nullopt + link.EnableVelocityChecks(ecm, false); + + EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm)); + EXPECT_EQ(std::nullopt, link.WorldAngularVelocity(ecm)); + EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm, offset)); + EXPECT_EQ(nullptr, ecm.Component(eLink)); + EXPECT_EQ(nullptr, ecm.Component(eLink)); + EXPECT_EQ(nullptr, ecm.Component(eLink)); } ////////////////////////////////////////////////// @@ -293,14 +314,25 @@ TEST_F(LinkIntegrationTest, LinkAccelerations) ASSERT_TRUE(link.Valid(ecm)); - // Before we add components, velocity functions should return nullopt + // Before we enable acceleration, acceleration should return nullopt EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm)); + // After enabling, they should return default values + link.EnableAccelerationChecks(ecm); + EXPECT_EQ(math::Vector3d::Zero, link.WorldLinearAcceleration(ecm)); + EXPECT_NE(nullptr, ecm.Component(eLink)); + + // After setting acceleration, we get the value math::Vector3d linAccel{1.0, 0.0, 0.0}; - math::Vector3d angAccel{0.0, 0.0, 2.0}; - ecm.CreateComponent(eLink, components::WorldLinearAcceleration(linAccel)); + ecm.SetComponentData(eLink, linAccel); EXPECT_EQ(linAccel, link.WorldLinearAcceleration(ecm)); + + // Disabling accelerations goes back to nullopt + link.EnableAccelerationChecks(ecm, false); + + EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eLink)); } ////////////////////////////////////////////////// diff --git a/test/plugins/MockSystem.cc b/test/plugins/MockSystem.cc index 6315624b76..d7602c5c38 100644 --- a/test/plugins/MockSystem.cc +++ b/test/plugins/MockSystem.cc @@ -3,6 +3,7 @@ #include IGNITION_ADD_PLUGIN(ignition::gazebo::MockSystem, ignition::gazebo::System, + ignition::gazebo::MockSystem::ISystemConfigure, ignition::gazebo::MockSystem::ISystemPreUpdate, ignition::gazebo::MockSystem::ISystemUpdate, ignition::gazebo::MockSystem::ISystemPostUpdate) diff --git a/test/plugins/MockSystem.hh b/test/plugins/MockSystem.hh index fee1048f3f..d6c3df31cb 100644 --- a/test/plugins/MockSystem.hh +++ b/test/plugins/MockSystem.hh @@ -25,10 +25,12 @@ namespace ignition { namespace gazebo { class IGNITION_GAZEBO_VISIBLE MockSystem : public gazebo::System, + public gazebo::ISystemConfigure, public gazebo::ISystemPreUpdate, public gazebo::ISystemUpdate, public gazebo::ISystemPostUpdate { + public: size_t configureCallCount {0}; public: size_t preUpdateCallCount {0}; public: size_t updateCallCount {0}; public: size_t postUpdateCallCount {0}; @@ -40,33 +42,47 @@ namespace ignition { std::function; + public: std::function &, + EntityComponentManager &, + EventManager &)> + configureCallback; public: CallbackType preUpdateCallback; public: CallbackType updateCallback; public: CallbackTypeConst postUpdateCallback; + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override final + { + ++this->configureCallCount; + if (this->configureCallback) + this->configureCallback(_entity, _sdf, _ecm, _eventMgr); + } public: void PreUpdate(const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &_manager) override final + gazebo::EntityComponentManager &_ecm) override final { ++this->preUpdateCallCount; if (this->preUpdateCallback) - this->preUpdateCallback(_info, _manager); + this->preUpdateCallback(_info, _ecm); } public: void Update(const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &_manager) override final + gazebo::EntityComponentManager &_ecm) override final { ++this->updateCallCount; if (this->updateCallback) - this->updateCallback(_info, _manager); + this->updateCallback(_info, _ecm); } public: void PostUpdate(const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_manager) override final + const gazebo::EntityComponentManager &_ecm) override final { ++this->postUpdateCallCount; if (this->postUpdateCallback) - this->postUpdateCallback(_info, _manager); + this->postUpdateCallback(_info, _ecm); } }; }