diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf new file mode 100644 index 0000000000..db3d48ca78 --- /dev/null +++ b/examples/worlds/shader_param.sdf @@ -0,0 +1,268 @@ + + + + + + + + + ogre2 + 0.8 0.8 0.8 + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + docked + + + + + + + docked + + + + + + RGB camera + floating + 350 + 315 + + camera + false + + + + Depth camera + floating + 350 + 315 + 500 + + depth_camera + false + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.6 0.6 0.6 1 + -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 + + + + + + + 0 0.0 0.5 0 0 0 + deformable_sphere + 3 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere + + + + true + 2.5 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + camera + + + 10 + depth_camera + + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + + + diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 9d90871a7b..de94818059 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -17,6 +17,11 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ #define IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ +#include + +#include +#include + #include #include #include @@ -27,11 +32,60 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class SdfElementSerializer + { + /// \brief Serialization for `sdf::Model`. + /// \param[in] _out Output stream. + /// \param[in] _elem Visual plugin elem to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::ElementPtr &_elem) + { + _out << "" + << "" + << _elem->ToString("") + << ""; + return _out; + } + + /// \brief Deserialization for `sdf::Element`. + /// \param[in] _in Input stream. + /// \param[out] _elem Visual plugin elem to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::ElementPtr &_elem) + { + std::string sdfStr(std::istreambuf_iterator(_in), {}); + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + bool result = sdf::readString(sdfStr, sdfParsed); + if (!result) + { + ignerr << "Unable to deserialize sdf::ElementPtr" << std::endl; + return _in; + } + + _elem = sdfParsed->Root()->GetFirstElement(); + return _in; + } + }; +} + namespace components { /// \brief A component that identifies an entity as being a visual. using Visual = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) + + /// \brief A component that contains a visual plugin's SDF element. + using VisualPlugin = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", + VisualPlugin) } } } diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 4407d68598..afbb751879 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -25,8 +25,11 @@ #include #include #include + #include #include +#include + #include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -211,6 +214,27 @@ namespace events IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief Event that notifies a visual plugin is to be loaded + class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \param[in] _elem Visual plugin SDF element + public: explicit VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem); + + /// \brief Get the entity to load the visual plugin for + public: ignition::gazebo::Entity Entity() const; + + /// \brief Get the sdf element of the visual plugin + public: sdf::ElementPtr Element() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh index d17b3922c5..683c66f529 100644 --- a/include/ignition/gazebo/rendering/Events.hh +++ b/include/ignition/gazebo/rendering/Events.hh @@ -32,6 +32,18 @@ namespace ignition /// more information about events. namespace events { + /// \brief The render event is emitted when the the scene manager is + /// updated with contents from the ECM. This event is emitted + /// before the PreRender event on the server side in the rendering + /// thread. It is also accessible on the GUI side. + /// + /// For example: + /// \code + /// eventManager.Emit(); + /// \endcode + using SceneUpdate = ignition::common::EventT; + /// \brief The render event is emitted before rendering updates. /// The event is emitted in the rendering thread so rendering /// calls can ben make in this event callback diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 6110b2083c..a757ec5378 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -214,6 +214,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _active True if active. public: void SetTransformActive(bool _active); + /// \brief Set the event manager to use + /// \param[in] _mgr Event manager to set to. + public: void SetEventManager(EventManager *_mgr); + /// \brief Private data pointer. private: std::unique_ptr dataPtr; }; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index ae98f41ec7..d54e1f377f 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -749,6 +749,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) components::Material(*_visual->Material())); } + // store the plugin in a component + if (_visual->Element()) + { + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + if (pluginElem) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugin(pluginElem)); + } + } + // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. this->dataPtr->newVisuals[visualEntity] = _visual->Element(); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index a52210425e..5da194e209 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -495,9 +495,6 @@ namespace ignition /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; - /// \brief Systems implementing Configure - private: std::vector systemsConfigure; - /// \brief Systems implementing PreUpdate private: std::vector systemsPreupdate; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index cc9b33161a..17641a873c 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1234,6 +1234,18 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) }); EXPECT_NE(kNullEntity, sensorId); + // Get visual entity + Entity visualId{kNullEntity}; + runner.EntityCompMgr().Each([&]( + const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Visual *_visual)->bool + { + EXPECT_NE(nullptr, _visual); + visualId = _entity; + return true; + }); + EXPECT_NE(kNullEntity, visualId); + // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; auto worldComponentId = ignition::common::hash64(worldComponentName); @@ -1258,6 +1270,14 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, sensorComponentId)); + // Check component registered by visual plugin + std::string visualComponentName{"VisualPluginComponent"}; + auto visualComponentId = ignition::common::hash64(visualComponentName); + + EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId)); + EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId, + visualComponentId)); + // Clang re-registers components between tests. If we don't unregister them // beforehand, the new plugin tries to create a storage type from a previous // plugin, causing a crash. @@ -1268,6 +1288,7 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) components::Factory::Instance()->Unregister(worldComponentId); components::Factory::Instance()->Unregister(modelComponentId); components::Factory::Instance()->Unregister(sensorComponentId); + components::Factory::Instance()->Unregister(visualComponentId); #endif } diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index ed195212ed..a89a5d8254 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -50,6 +50,15 @@ class ignition::gazebo::gui::events::ModelEditorAddEntity::Implementation public: ignition::gazebo::Entity parent; }; +class ignition::gazebo::gui::events::VisualPlugin::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf element of the visual plugin + public: sdf::ElementPtr element; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -132,3 +141,24 @@ QMap &ModelEditorAddEntity::Data() { return this->dataPtr->data; } + +///////////////////////////////////////////////// +VisualPlugin::VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem) : + QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->element = _elem; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPlugin::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +sdf::ElementPtr VisualPlugin::Element() const +{ + return this->dataPtr->element; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 50a2c3f075..2a70c033ac 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include @@ -28,7 +32,9 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include #include "ignition/gazebo/gui/GuiSystem.hh" +#include "ignition/gazebo/SystemLoader.hh" #include "GuiRunner.hh" @@ -68,6 +74,31 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Name of WorldControl service public: std::string controlService; + + /// \brief System loader for loading ign-gazebo systems + public: std::unique_ptr systemLoader; + + /// \brief Mutex to protect systemLoader + public: std::mutex systemLoadMutex; + + /// \brief Events containing visual plugins to load + public: std::vector> + visualPlugins; + + /// \brief Systems implementing PreUpdate + public: std::vector systems; + + /// \brief Systems implementing PreUpdate + public: std::vector systemsPreupdate; + + /// \brief Systems implementing Update + public: std::vector systemsUpdate; + + /// \brief Systems implementing PostUpdate + public: std::vector systemsPostupdate; + + /// \brief Manager of all events. + public: EventManager eventMgr; }; ///////////////////////////////////////////////// @@ -92,7 +123,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) // so that an offset is not required this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2); - auto win = gui::App()->findChild(); + auto win = ignition::gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); win->setProperty("worldNames", winWorldNames); @@ -136,7 +167,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == ignition::gui::events::WorldControl::kType) { auto worldControlEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (worldControlEvent) { msgs::WorldControlState req; @@ -162,6 +193,20 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) + { + auto visualPluginEvent = + reinterpret_cast(_event); + if (visualPluginEvent) + { + std::lock_guard lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + sdf::ElementPtr pluginElem = visualPluginEvent->Element(); + this->dataPtr->visualPlugins.push_back( + std::make_pair(entity, pluginElem)); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -171,7 +216,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) void GuiRunner::RequestState() { // set up service for async state response callback - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv); @@ -225,7 +270,7 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) // todo(anyone) store reqSrv string in a member variable and use it here // and in RequestState() - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; this->dataPtr->node.UnadvertiseSrv(reqSrv); @@ -264,7 +309,8 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) ///////////////////////////////////////////////// void GuiRunner::UpdatePlugins() { - auto plugins = gui::App()->findChildren(); + // gui plugins + auto plugins = ignition::gui::App()->findChildren(); for (auto plugin : plugins) { plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); @@ -272,4 +318,92 @@ void GuiRunner::UpdatePlugins() this->dataPtr->ecm.ClearRemovedComponents(); this->dataPtr->ecm.ClearNewlyCreatedEntities(); this->dataPtr->ecm.ProcessRemoveEntityRequests(); + + // ign-gazebo systems + this->LoadSystems(); + this->UpdateSystems(); +} + +///////////////////////////////////////////////// +void GuiRunner::LoadSystems() +{ + std::lock_guard lock(this->dataPtr->systemLoadMutex); + // currently only support systems that are visual plugins + for (auto &visualPlugin : this->dataPtr->visualPlugins) + { + Entity entity = visualPlugin.first; + sdf::ElementPtr pluginElem = visualPlugin.second; + auto filename = pluginElem->Get("filename"); + auto name = pluginElem->Get("name"); + if (filename != "__default__" && name != "__default__") + { + std::optional system; + if (!this->dataPtr->systemLoader) + this->dataPtr->systemLoader = std::make_unique(); + system = this->dataPtr->systemLoader->LoadPlugin( + filename, name, pluginElem); + if (system) + { + SystemPluginPtr sys = system.value(); + this->dataPtr->systems.push_back(sys); + this->dataPtr->systemsPreupdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsUpdate.push_back( + sys->QueryInterface()); + this->dataPtr->systemsPostupdate.push_back( + sys->QueryInterface()); + + auto sysConfigure = sys->QueryInterface(); + if (sysConfigure) + { + sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm, + this->dataPtr->eventMgr); + } + igndbg << "Loaded system [" << name + << "] for entity [" << entity << "] in GUI" + << std::endl; + } + } + } + this->dataPtr->visualPlugins.clear(); +} + +///////////////////////////////////////////////// +void GuiRunner::UpdateSystems() +{ + IGN_PROFILE("GuiRunner::UpdateSystems"); + + { + IGN_PROFILE("PreUpdate"); + for (auto& system : this->dataPtr->systemsPreupdate) + { + if (system) + system->PreUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("Update"); + for (auto& system : this->dataPtr->systemsUpdate) + { + if (system) + system->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("PostUpdate"); + // \todo(anyone) Do PostUpdates in parallel + for (auto& system : this->dataPtr->systemsPostupdate) + { + if (system) + system->PostUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } +} + +///////////////////////////////////////////////// +EventManager &GuiRunner::GuiEventManager() const +{ + return this->dataPtr->eventMgr; } diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index 1a06098161..bc5a730553 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -25,6 +25,7 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -46,6 +47,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Destructor public: ~GuiRunner() override; + /// \brief Get the event manager for the gui + public: EventManager &GuiEventManager() const; + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; @@ -74,6 +78,12 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 private: Q_INVOKABLE void UpdatePlugins(); + /// \brief Load systems + private: void LoadSystems(); + + /// \brief Update systems + private: void UpdateSystems(); + /// \brief Pointer to private data. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index cccb6caaa4..48c0735f10 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -14,10 +14,15 @@ * limitations under the License. * */ + +#include #include +#include "../../GuiRunner.hh" #include "GzSceneManager.hh" +#include + #include #include #include @@ -28,6 +33,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -58,6 +64,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Mutex to protect gui event and system upate call race conditions /// for newEntities and removedEntities public: std::mutex newRemovedEntityMutex; + + /// \brief Indicates whether initial visual plugins have been loaded or not. + public: bool initializedVisualPlugins = false; }; } } @@ -102,6 +111,42 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + // load visual plugin on gui side + std::map pluginElems; + if (!this->dataPtr->initializedVisualPlugins) + { + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + this->dataPtr->initializedVisualPlugins = true; + } + else + { + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + } + for (const auto &it : pluginElems) + { + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, it.second); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &visualPluginEvent); + } + // Emit entities created / removed event for gui::Plugins which don't have // direct access to the ECM. std::set created; @@ -163,6 +208,16 @@ void GzSceneManagerPrivate::OnRender() return; this->renderUtil.SetScene(this->scene); + + auto runners = ignition::gui::App()->findChildren(); + if (runners.empty() || runners[0] == nullptr) + { + ignerr << "Internal error: no GuiRunner found." << std::endl; + } + else + { + this->renderUtil.SetEventManager(&runners[0]->GuiEventManager()); + } } this->renderUtil.Update(); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 3df544c588..0679db3769 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -89,6 +89,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/Events.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/SceneManager.hh" #include "ignition/gazebo/rendering/MarkerManager.hh" @@ -193,6 +194,9 @@ class ignition::gazebo::RenderUtilPrivate const components::Name *_name, const components::ParentEntity *_parent); + /// \brief Event manager used for emitting render / scene events + public: EventManager *eventManager{nullptr}; + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1583,6 +1587,9 @@ void RenderUtil::Update() } } } + + if (this->dataPtr->eventManager) + this->dataPtr->eventManager->Emit(); } ////////////////////////////////////////////////// @@ -3637,3 +3644,9 @@ void RenderUtilPrivate::CreateLight( this->newLights.push_back(std::make_tuple(_entity, _light->Data(), _name->Data(), _parent->Data())); } + +////////////////////////////////////////////////// +void RenderUtil::SetEventManager(EventManager *_mgr) +{ + this->dataPtr->eventManager = _mgr; +} diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 17514029f8..5054dad438 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -128,6 +128,7 @@ add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) +add_subdirectory(shader_param) add_subdirectory(thermal) add_subdirectory(thruster) add_subdirectory(touch_plugin) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 5d802189b1..d5309a82c7 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -242,7 +242,6 @@ void SensorsPrivate::RunOnce() this->renderUtil.Update(); } - if (!this->activeSensors.empty()) { this->sensorMaskMutex.lock(); @@ -422,8 +421,9 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->renderUtil.SetEnableSensors(true, std::bind(&Sensors::CreateSensor, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - this->dataPtr->renderUtil.SetRemoveSensorCb( + this->dataPtr->renderUtil.SetRemoveSensorCb( std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1)); + this->dataPtr->renderUtil.SetEventManager(&_eventMgr); // parse sensor-specific data auto worldEntity = _ecm.EntityByComponents(components::World()); diff --git a/src/systems/shader_param/CMakeLists.txt b/src/systems/shader_param/CMakeLists.txt new file mode 100644 index 0000000000..0d14af33f3 --- /dev/null +++ b/src/systems/shader_param/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(shader-param + SOURCES + ShaderParam.cc + PUBLIC_LINK_LIBS + ${rendering_target} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc new file mode 100644 index 0000000000..feab7709b2 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.cc @@ -0,0 +1,396 @@ +/* + * Copyright (C) 2022 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 "ShaderParam.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::ShaderParamPrivate +{ + /// \brief Data structure for storing shader param info + public: class ShaderParamValue + { + /// \brief shader type: vertex or fragment + public: std::string shader; + + /// \brief variable type: int, float, float_array, int_array + /// \todo(anyone) support samplers + public: std::string type; + + /// \brief variable name of param + public: std::string name; + + /// \brief param value + public: std::string value; + }; + + /// \brief Path to vertex shader + public: std::string vertexShaderUri; + + /// \brief Path to fragment shader + public: std::string fragmentShaderUri; + + /// \brief Mutex to protect sim time updates. + public: std::mutex mutex; + + /// \brief Connection to pre-render event callback + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Name of visual this plugin is attached to + public: std::string visualName; + + /// \brief Pointer to visual + public: rendering::VisualPtr visual; + + /// \brief Material used by this visual + public: rendering::MaterialPtr material; + + /// \brief Pointer to scene + public: rendering::ScenePtr scene; + + /// \brief Entity id of the visual + public: Entity entity = kNullEntity; + + /// \brief A list of shader params + public: std::vector shaderParams; + + /// \brief Time params that will be updated every iteration + public: std::vector timeParams; + + /// \brief Current sim time + public: std::chrono::steady_clock::duration currentSimTime; + + /// \brief All rendering operations must happen within this call + public: void OnUpdate(); +}; + +///////////////////////////////////////////////// +ShaderParam::ShaderParam() + : System(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ShaderParam::~ShaderParam() +{ +} + +///////////////////////////////////////////////// +void ShaderParam::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + IGN_PROFILE("ShaderParam::Configure"); + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto sdf = const_cast(_sdf.get()); + + if (sdf->HasElement("param")) + { + // loop and parse all shader params + sdf::ElementPtr paramElem = sdf->GetElement("param"); + while (paramElem) + { + if (!paramElem->HasElement("shader") || + !paramElem->HasElement("name")) + { + ignerr << " must have and sdf elements" + << std::endl; + paramElem = paramElem->GetNextElement("param"); + continue; + } + std::string shaderType = paramElem->Get("shader"); + std::string paramName = paramElem->Get("name"); + + std::string type = paramElem->Get("type", "float").first; + std::string value = paramElem->Get("value", "").first; + + ShaderParamPrivate::ShaderParamValue spv; + spv.shader = shaderType; + spv.name = paramName; + spv.value = value; + spv.type = type; + this->dataPtr->shaderParams.push_back(spv); + paramElem = paramElem->GetNextElement("param"); + } + } + + // parse path to shaders + if (sdf->HasElement("shader")) + { + sdf::ElementPtr shaderElem = sdf->GetElement("shader"); + if (!shaderElem->HasElement("vertex") || + !shaderElem->HasElement("fragment")) + { + ignerr << " must have and sdf elements" + << std::endl; + } + else + { + auto modelEntity = topLevelModel(_entity, _ecm); + auto modelPath = + _ecm.ComponentData(modelEntity); + sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); + this->dataPtr->vertexShaderUri = common::findFile( + asFullPath(vertexElem->Get(), modelPath.value())); + sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); + this->dataPtr->fragmentShaderUri = common::findFile( + asFullPath(fragmentElem->Get(), modelPath.value())); + } + } + + this->dataPtr->entity = _entity; + auto nameComp = _ecm.Component(_entity); + this->dataPtr->visualName = nameComp->Data(); + + // connect to the SceneUpdate event + // the callback is executed in the rendering thread so do all + // rendering operations in that thread + this->dataPtr->connection = + _eventMgr.Connect( + std::bind(&ShaderParamPrivate::OnUpdate, this->dataPtr.get())); +} + +////////////////////////////////////////////////// +void ShaderParam::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &) +{ + IGN_PROFILE("ShaderParam::PreUpdate"); + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->currentSimTime = _info.simTime; +} + +////////////////////////////////////////////////// +void ShaderParamPrivate::OnUpdate() +{ + std::lock_guard lock(this->mutex); + if (this->visualName.empty()) + return; + + if (!this->scene) + this->scene = rendering::sceneFromFirstRenderEngine(); + + if (!this->scene) + return; + + if (!this->visual) + { + // this does a breadth first search for visual with the entity id + // \todo(anyone) provide a helper function in RenderUtil to search for + // visual by entity id? + auto rootVis = scene->RootVisual(); + std::list nodes; + nodes.push_back(rootVis); + while (!nodes.empty()) + { + auto n = nodes.front(); + nodes.pop_front(); + if (n && n->HasUserData("gazebo-entity")) + { + // RenderUti stores gazebo-entity user data as int + // \todo(anyone) Change this to uint64_t in Ignition H? + auto variant = n->UserData("gazebo-entity"); + const int *value = std::get_if(&variant); + if (value && *value == static_cast(this->entity)) + { + this->visual = std::dynamic_pointer_cast(n); + break; + } + } + for (unsigned int i = 0; i < n->ChildCount(); ++i) + nodes.push_back(n->ChildByIndex(i)); + } + } + + if (!this->visual) + return; + + // get the material and set shaders + if (!this->material) + { + auto mat = scene->CreateMaterial(); + mat->SetVertexShader(this->vertexShaderUri); + mat->SetFragmentShader(this->fragmentShaderUri); + this->visual->SetMaterial(mat); + scene->DestroyMaterial(mat); + this->material = this->visual->Material(); + } + + if (!this->material) + return; + + // set the shader params read from SDF + // this is only done once + for (const auto & spv : this->shaderParams) + { + std::vector values = common::split(spv.value, " "); + + int intValue = 0; + float floatValue = 0; + std::vector floatArrayValue; + + rendering::ShaderParam::ParamType paramType = + rendering::ShaderParam::PARAM_NONE; + + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + { + params = this->material->FragmentShaderParams(); + } + else if (spv.shader == "vertex") + { + params = this->material->VertexShaderParams(); + } + + // if no is specified, this could be a constant + if (values.empty()) + { + // \todo handle args for constants + (*params)[spv.name] = intValue; + } + // float / int + else if (values.size() == 1u) + { + std::string str = values[0]; + + // TIME is reserved keyword for sim time + if (str == "TIME") + { + this->timeParams.push_back(spv); + continue; + } + + // if is not empty, respect the specified type + if (!spv.type.empty()) + { + if (spv.type == "int") + { + intValue = std::stoi(str); + paramType = rendering::ShaderParam::PARAM_INT; + } + else if (spv.type == "float") + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + else + { + // \todo(anyone) support texture samplers + } + } + // else do our best guess at what the type is + else + { + std::string::size_type sz; + int n = std::stoi(str, &sz); + if ( sz == str.size()) + { + intValue = n; + paramType = rendering::ShaderParam::PARAM_INT; + } + else + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + } + } + // arrays + else + { + // int array + if (!spv.type.empty() && spv.type == "int_array") + { + for (const auto &v : values) + floatArrayValue.push_back(std::stoi(v)); + paramType = rendering::ShaderParam::PARAM_INT_BUFFER; + } + // treat everything else as float_array + else + { + for (const auto &v : values) + floatArrayValue.push_back(std::stof(v)); + paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; + } + } + + // set the params + if (paramType == rendering::ShaderParam::PARAM_INT) + { + (*params)[spv.name] = intValue; + } + else if (paramType == rendering::ShaderParam::PARAM_FLOAT) + { + (*params)[spv.name] = floatValue; + } + else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || + paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) + { + (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); + float *fv = &floatArrayValue[0]; + (*params)[spv.name].UpdateBuffer(fv); + } + } + this->shaderParams.clear(); + + // time variables need to be updated every iteration + for (const auto & spv : this->timeParams) + { + float floatValue = (std::chrono::duration_cast( + this->currentSimTime).count()) * 1e-9; + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + params = this->material->FragmentShaderParams(); + else if (spv.shader == "vertex") + params = this->material->VertexShaderParams(); + (*params)[spv.name] = floatValue; + } +} + +IGNITION_ADD_PLUGIN(ShaderParam, + ignition::gazebo::System, + ShaderParam::ISystemConfigure, + ShaderParam::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ShaderParam, + "ignition::gazebo::systems::ShaderParam") diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh new file mode 100644 index 0000000000..d77e8bfd78 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 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_SYSTEMS_SHADER_PARAM_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ShaderParamPrivate; + + /// \brief A plugin for setting shaders to a visual and its params + /// + /// Plugin parameters: + /// + /// + /// Path to vertex program + /// Path to fragment program + /// Shader parameter - can be repeated within plugin SDF element + /// Name of uniform variable bound to the shader + /// Type of shader, i.e. vertex, fragment + /// Variable type: float, int, float_array, int_array + /// Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be 'TIME', in which case the value will + /// be bound to sim time. + /// + /// Example usage: + /// + /// \verbatim + /// + /// + /// materials/my_vs.glsl + /// materials/my_fs.glsl + /// + /// + /// + /// ambient + /// fragment + /// float_array + /// 1.0 0.0 0.0 1.0 + /// + /// + /// \endverbatim + class ShaderParam + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ShaderParam(); + + /// \brief Destructor + public: ~ShaderParam() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6b712785a9..77197e73c3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -76,6 +76,7 @@ set(tests_needing_display optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc + shader_param_system.cc thermal_system.cc thermal_sensor_system.cc ) diff --git a/test/integration/shader_param_system.cc b/test/integration/shader_param_system.cc new file mode 100644 index 0000000000..906133e937 --- /dev/null +++ b/test/integration/shader_param_system.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test ShaderParamTest system +class ShaderParamTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + mutex.lock(); + unsigned int channels = 3u; + unsigned int imageSamples = _msg.width() * _msg.height(); + unsigned int imageBufferSize = imageSamples * sizeof(unsigned char) + * channels; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples * channels]; + memcpy(imageBuffer, _msg.data().c_str(), imageBufferSize); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks camera image data to verify that the sphere is using +// custom material shaders +TEST_F(ShaderParamTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ShaderParam)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "shader_param.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the image camera topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + // Run server and verify that we are receiving a message + // from the image camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + // shaders set the sphere color to red + unsigned int height = 320; + unsigned int width = 240; + + int mid = (height / 2 * width * 3u) + (width / 2 - 1) * 3u; + + // Lock access to buffer and don't release it + mutex.lock(); + int r = static_cast(imageBuffer[mid]); + int g = static_cast(imageBuffer[mid+1]); + int b = static_cast(imageBuffer[mid+2]); + EXPECT_GT(r, g); + EXPECT_GT(r, b); + EXPECT_EQ(g, b); + + delete[] imageBuffer; +} diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 19306775b4..554589094a 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -13,6 +13,7 @@ set (test_plugins EventTriggerSystem TestModelSystem TestSensorSystem + TestVisualSystem TestWorldSystem MockSystem Null diff --git a/test/plugins/TestVisualSystem.cc b/test/plugins/TestVisualSystem.cc new file mode 100644 index 0000000000..9cc3741117 --- /dev/null +++ b/test/plugins/TestVisualSystem.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2022 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 "TestVisualSystem.hh" + +#include + +IGNITION_ADD_PLUGIN(ignition::gazebo::TestVisualSystem, + ignition::gazebo::System, + ignition::gazebo::TestVisualSystem::ISystemConfigure) diff --git a/test/plugins/TestVisualSystem.hh b/test/plugins/TestVisualSystem.hh new file mode 100644 index 0000000000..e8cdf4a45f --- /dev/null +++ b/test/plugins/TestVisualSystem.hh @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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_TEST_TESTVISUALSYSTEM_HH_ +#define IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ +using VisualPluginComponent = components::Component; +IGN_GAZEBO_REGISTER_COMPONENT("VisualPluginComponent", + VisualPluginComponent) +} +} + +class TestVisualSystem : + public System, + public ISystemConfigure +{ + public: TestVisualSystem() + { + igndbg << "Constructing TestVisualSystem" << std::endl; + } + + public: ~TestVisualSystem() + { + igndbg << "Destroying TestVisualSystem" << std::endl; + } + + private: bool Service(msgs::StringMsg &_msg) + { + igndbg << "TestVisualSystem service called" << std::endl; + _msg.set_data("TestVisualSystem"); + return true; + } + + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventManager*/) override + { + igndbg << "Configuring TestVisualSystem" << std::endl; + auto value = _sdf->Get("visual_key"); + _ecm.CreateComponent(_entity, + components::VisualPluginComponent(value)); + + // Create a test service + this->node.Advertise("/test/service/visual", + &TestVisualSystem::Service, this); + } + + private: transport::Node node; +}; +} +} + +#endif diff --git a/test/worlds/plugins.sdf b/test/worlds/plugins.sdf index 466215d186..d4668e294d 100644 --- a/test/worlds/plugins.sdf +++ b/test/worlds/plugins.sdf @@ -25,6 +25,13 @@ 456 + + + 890 + +