From 3a05520c73338aacb7bf6d5d98b26a074654d09e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 7 May 2021 18:13:32 -0700 Subject: [PATCH 01/23] Split transport scene manager into a plugin outside Scene3D Signed-off-by: Louise Poubel --- examples/config/scene3d.config | 8 +- src/plugins/CMakeLists.txt | 1 + src/plugins/scene3d/Scene3D.cc | 757 +---------------- .../transport_scene_manager/CMakeLists.txt | 11 + .../transport_scene_manager/Scene3D_TEST.cc | 122 +++ .../TransportSceneManager.cc | 789 ++++++++++++++++++ .../TransportSceneManager.hh | 59 ++ .../TransportSceneManager.qml | 26 + .../TransportSceneManager.qrc | 5 + 9 files changed, 1020 insertions(+), 758 deletions(-) create mode 100644 src/plugins/transport_scene_manager/CMakeLists.txt create mode 100644 src/plugins/transport_scene_manager/Scene3D_TEST.cc create mode 100644 src/plugins/transport_scene_manager/TransportSceneManager.cc create mode 100644 src/plugins/transport_scene_manager/TransportSceneManager.hh create mode 100644 src/plugins/transport_scene_manager/TransportSceneManager.qml create mode 100644 src/plugins/transport_scene_manager/TransportSceneManager.qrc diff --git a/examples/config/scene3d.config b/examples/config/scene3d.config index 08e5e3e75..de1862c1a 100644 --- a/examples/config/scene3d.config +++ b/examples/config/scene3d.config @@ -15,9 +15,11 @@ 0.8 0.8 0.8 -6 0 6 0 0.5 0 - - ogre - scene + + /example/scene + /example/pose + /example/delete + /example/scene - - Controls diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 0c0fbd707..ebbe84fd2 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -119,6 +119,7 @@ add_subdirectory(image_display) add_subdirectory(key_publisher) add_subdirectory(plotting) add_subdirectory(publisher) +add_subdirectory(minimal_scene) add_subdirectory(scene3d) add_subdirectory(screenshot) add_subdirectory(topic_echo) diff --git a/src/plugins/minimal_scene/CMakeLists.txt b/src/plugins/minimal_scene/CMakeLists.txt new file mode 100644 index 000000000..9843181db --- /dev/null +++ b/src/plugins/minimal_scene/CMakeLists.txt @@ -0,0 +1,11 @@ +ign_gui_add_plugin(MinimalScene + SOURCES + MinimalScene.cc + QT_HEADERS + MinimalScene.hh + TEST_SOURCES + # MinimalScene_TEST.cc + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) + diff --git a/src/plugins/minimal_scene/MinimalScene.cc b/src/plugins/minimal_scene/MinimalScene.cc new file mode 100644 index 000000000..d003d4f54 --- /dev/null +++ b/src/plugins/minimal_scene/MinimalScene.cc @@ -0,0 +1,796 @@ +/* + * Copyright (C) 2017 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 "MinimalScene.hh" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +// TODO(louise) Remove these pragmas once ign-rendering +// is disabling the warnings +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#include +#include +#include +#include +#include + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "ignition/gui/Application.hh" +#include "ignition/gui/Conversions.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/MainWindow.hh" + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + /// \brief Private data class for IgnRenderer + class IgnRendererPrivate + { + /// \brief Flag to indicate if mouse event is dirty + public: bool mouseDirty = false; + + /// \brief Mouse event + public: common::MouseEvent mouseEvent; + + /// \brief Mouse move distance since last event. + public: math::Vector2d drag; + + /// \brief Mutex to protect mouse events + public: std::mutex mutex; + + /// \brief User camera + public: rendering::CameraPtr camera; + + /// \brief Camera orbit controller + public: rendering::OrbitViewController viewControl; + + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery; + + /// \brief View control focus target + public: math::Vector3d target; + }; + + /// \brief Private data class for RenderWindowItem + class RenderWindowItemPrivate + { + /// \brief Keep latest mouse event + public: common::MouseEvent mouseEvent; + + /// \brief Render thread + public : RenderThread *renderThread = nullptr; + + //// \brief List of threads + public: static QList threads; + }; + + /// \brief Private data class for MinimalScene + class MinimalScenePrivate + { + }; +} +} +} + +using namespace ignition; +using namespace gui; +using namespace plugins; + +QList RenderWindowItemPrivate::threads; + +///////////////////////////////////////////////// +IgnRenderer::IgnRenderer() + : dataPtr(new IgnRendererPrivate) +{ +} + + +///////////////////////////////////////////////// +IgnRenderer::~IgnRenderer() +{ +} + +///////////////////////////////////////////////// +void IgnRenderer::Render() +{ + if (this->textureDirty) + { + this->dataPtr->camera->SetImageWidth(this->textureSize.width()); + this->dataPtr->camera->SetImageHeight(this->textureSize.height()); + this->dataPtr->camera->SetAspectRatio(this->textureSize.width() / + this->textureSize.height()); + // setting the size should cause the render texture to be rebuilt + this->dataPtr->camera->PreRender(); + this->textureDirty = false; + } + + this->textureId = this->dataPtr->camera->RenderTextureGLId(); + + // view control + this->HandleMouseEvent(); + + // update and render to texture + this->dataPtr->camera->Update(); + + if (ignition::gui::App()) + { + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + new gui::events::Render()); + } +} + +///////////////////////////////////////////////// +void IgnRenderer::HandleMouseEvent() +{ + std::lock_guard lock(this->dataPtr->mutex); + if (!this->dataPtr->mouseDirty) + return; + + this->dataPtr->viewControl.SetCamera(this->dataPtr->camera); + + if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::SCROLL) + { + this->dataPtr->target = + this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); + this->dataPtr->viewControl.SetTarget(this->dataPtr->target); + double distance = this->dataPtr->camera->WorldPosition().Distance( + this->dataPtr->target); + double amount = -this->dataPtr->drag.Y() * distance / 5.0; + this->dataPtr->viewControl.Zoom(amount); + } + else + { + if (this->dataPtr->drag == math::Vector2d::Zero) + { + this->dataPtr->target = this->ScreenToScene( + this->dataPtr->mouseEvent.PressPos()); + this->dataPtr->viewControl.SetTarget(this->dataPtr->target); + } + + // Pan with left button + if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) + { + if (Qt::ShiftModifier == QGuiApplication::queryKeyboardModifiers()) + this->dataPtr->viewControl.Orbit(this->dataPtr->drag); + else + this->dataPtr->viewControl.Pan(this->dataPtr->drag); + } + // Orbit with middle button + else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::MIDDLE) + { + this->dataPtr->viewControl.Orbit(this->dataPtr->drag); + } + else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::RIGHT) + { + double hfov = this->dataPtr->camera->HFOV().Radian(); + double vfov = 2.0f * atan(tan(hfov / 2.0f) / + this->dataPtr->camera->AspectRatio()); + double distance = this->dataPtr->camera->WorldPosition().Distance( + this->dataPtr->target); + double amount = ((-this->dataPtr->drag.Y() / + static_cast(this->dataPtr->camera->ImageHeight())) + * distance * tan(vfov/2.0) * 6.0); + this->dataPtr->viewControl.Zoom(amount); + } + } + this->dataPtr->drag = 0; + this->dataPtr->mouseDirty = false; +} + +///////////////////////////////////////////////// +void IgnRenderer::Initialize() +{ + if (this->initialized) + return; + + std::map params; + params["useCurrentGLContext"] = "1"; + auto engine = rendering::engine(this->engineName, params); + if (!engine) + { + ignerr << "Engine [" << this->engineName << "] is not supported" + << std::endl; + return; + } + + // Scene + auto scene = engine->SceneByName(this->sceneName); + if (!scene) + { + igndbg << "Create scene [" << this->sceneName << "]" << std::endl; + scene = engine->CreateScene(this->sceneName); + scene->SetAmbientLight(this->ambientLight); + scene->SetBackgroundColor(this->backgroundColor); + } + + if (this->skyEnable) + { + scene->SetSkyEnabled(true); + } + + auto root = scene->RootVisual(); + + // Camera + this->dataPtr->camera = scene->CreateCamera(); + root->AddChild(this->dataPtr->camera); + this->dataPtr->camera->SetLocalPose(this->cameraPose); + this->dataPtr->camera->SetImageWidth(this->textureSize.width()); + this->dataPtr->camera->SetImageHeight(this->textureSize.height()); + this->dataPtr->camera->SetAntiAliasing(8); + this->dataPtr->camera->SetHFOV(M_PI * 0.5); + // setting the size and calling PreRender should cause the render texture to + // be rebuilt + this->dataPtr->camera->PreRender(); + this->textureId = this->dataPtr->camera->RenderTextureGLId(); + + // Ray Query + this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); + + this->initialized = true; +} + +///////////////////////////////////////////////// +void IgnRenderer::Destroy() +{ + auto engine = rendering::engine(this->engineName); + if (!engine) + return; + auto scene = engine->SceneByName(this->sceneName); + if (!scene) + return; + scene->DestroySensor(this->dataPtr->camera); + + // If that was the last sensor, destroy scene + if (scene->SensorCount() == 0) + { + igndbg << "Destroy scene [" << scene->Name() << "]" << std::endl; + engine->DestroyScene(scene); + + // TODO(anyone) If that was the last scene, terminate engine? + } +} + +///////////////////////////////////////////////// +void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, + const math::Vector2d &_drag) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->mouseEvent = _e; + this->dataPtr->drag += _drag; + this->dataPtr->mouseDirty = true; +} + +///////////////////////////////////////////////// +math::Vector3d IgnRenderer::ScreenToScene( + const math::Vector2i &_screenPos) const +{ + // Normalize point on the image + double width = this->dataPtr->camera->ImageWidth(); + double height = this->dataPtr->camera->ImageHeight(); + + double nx = 2.0 * _screenPos.X() / width - 1.0; + double ny = 1.0 - 2.0 * _screenPos.Y() / height; + + // Make a ray query + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + + auto result = this->dataPtr->rayQuery->ClosestPoint(); + if (result) + return result.point; + + // Set point to be 10m away if no intersection found + return this->dataPtr->rayQuery->Origin() + + this->dataPtr->rayQuery->Direction() * 10; +} + +///////////////////////////////////////////////// +RenderThread::RenderThread() +{ + RenderWindowItemPrivate::threads << this; +} + +///////////////////////////////////////////////// +void RenderThread::RenderNext() +{ + this->context->makeCurrent(this->surface); + + if (!this->ignRenderer.initialized) + { + // Initialize renderer + this->ignRenderer.Initialize(); + } + + // check if engine has been successfully initialized + if (!this->ignRenderer.initialized) + { + ignerr << "Unable to initialize renderer" << std::endl; + return; + } + + this->ignRenderer.Render(); + + emit TextureReady(this->ignRenderer.textureId, this->ignRenderer.textureSize); +} + +///////////////////////////////////////////////// +void RenderThread::ShutDown() +{ + this->context->makeCurrent(this->surface); + + this->ignRenderer.Destroy(); + + this->context->doneCurrent(); + delete this->context; + + // schedule this to be deleted only after we're done cleaning up + this->surface->deleteLater(); + + // Stop event processing, move the thread to GUI and make sure it is deleted. + this->moveToThread(QGuiApplication::instance()->thread()); +} + + +///////////////////////////////////////////////// +void RenderThread::SizeChanged() +{ + auto item = qobject_cast(this->sender()); + if (!item) + { + ignerr << "Internal error, sender is not QQuickItem." << std::endl; + return; + } + + if (item->width() <= 0 || item->height() <= 0) + return; + + this->ignRenderer.textureSize = QSize(item->width(), item->height()); + this->ignRenderer.textureDirty = true; +} + +///////////////////////////////////////////////// +TextureNode::TextureNode(QQuickWindow *_window) + : window(_window) +{ + // Our texture node must have a texture, so use the default 0 texture. +#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) + this->texture = this->window->createTextureFromId(0, QSize(1, 1)); +#else + void * nativeLayout; + this->texture = this->window->createTextureFromNativeObject( + QQuickWindow::NativeObjectTexture, &nativeLayout, 0, QSize(1, 1), + QQuickWindow::TextureIsOpaque); +#endif + this->setTexture(this->texture); +} + +///////////////////////////////////////////////// +TextureNode::~TextureNode() +{ + delete this->texture; +} + +///////////////////////////////////////////////// +void TextureNode::NewTexture(int _id, const QSize &_size) +{ + this->mutex.lock(); + this->id = _id; + this->size = _size; + this->mutex.unlock(); + + // We cannot call QQuickWindow::update directly here, as this is only allowed + // from the rendering thread or GUI thread. + emit PendingNewTexture(); +} + +///////////////////////////////////////////////// +void TextureNode::PrepareNode() +{ + this->mutex.lock(); + int newId = this->id; + QSize sz = this->size; + this->id = 0; + this->mutex.unlock(); + if (newId) + { + delete this->texture; + // note: include QQuickWindow::TextureHasAlphaChannel if the rendered + // content has alpha. +#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) + this->texture = this->window->createTextureFromId( + newId, sz, QQuickWindow::TextureIsOpaque); +#else + // TODO(anyone) Use createTextureFromNativeObject + // https://github.com/ignitionrobotics/ign-gui/issues/113 +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + this->texture = this->window->createTextureFromId( + newId, sz, QQuickWindow::TextureIsOpaque); +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#endif + this->setTexture(this->texture); + + this->markDirty(DirtyMaterial); + + // This will notify the rendering thread that the texture is now being + // rendered and it can start rendering to the other one. + emit TextureInUse(); + } +} + +///////////////////////////////////////////////// +RenderWindowItem::RenderWindowItem(QQuickItem *_parent) + : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) +{ + this->setAcceptedMouseButtons(Qt::AllButtons); + this->setFlag(ItemHasContents); + this->dataPtr->renderThread = new RenderThread(); +} + +///////////////////////////////////////////////// +RenderWindowItem::~RenderWindowItem() +{ +} + +///////////////////////////////////////////////// +void RenderWindowItem::Ready() +{ + this->dataPtr->renderThread->surface = new QOffscreenSurface(); + this->dataPtr->renderThread->surface->setFormat( + this->dataPtr->renderThread->context->format()); + this->dataPtr->renderThread->surface->create(); + + this->dataPtr->renderThread->ignRenderer.textureSize = + QSize(std::max({this->width(), 1.0}), std::max({this->height(), 1.0})); + + this->dataPtr->renderThread->moveToThread(this->dataPtr->renderThread); + + this->connect(this, &QObject::destroyed, + this->dataPtr->renderThread, &RenderThread::ShutDown, + Qt::QueuedConnection); + + this->connect(this, &QQuickItem::widthChanged, + this->dataPtr->renderThread, &RenderThread::SizeChanged); + this->connect(this, &QQuickItem::heightChanged, + this->dataPtr->renderThread, &RenderThread::SizeChanged); + + this->dataPtr->renderThread->start(); + this->update(); +} + +///////////////////////////////////////////////// +QSGNode *RenderWindowItem::updatePaintNode(QSGNode *_node, + QQuickItem::UpdatePaintNodeData * /*_data*/) +{ + TextureNode *node = static_cast(_node); + + if (!this->dataPtr->renderThread->context) + { + QOpenGLContext *current = this->window()->openglContext(); + // Some GL implementations require that the currently bound context is + // made non-current before we set up sharing, so we doneCurrent here + // and makeCurrent down below while setting up our own context. + current->doneCurrent(); + + this->dataPtr->renderThread->context = new QOpenGLContext(); + this->dataPtr->renderThread->context->setFormat(current->format()); + this->dataPtr->renderThread->context->setShareContext(current); + this->dataPtr->renderThread->context->create(); + this->dataPtr->renderThread->context->moveToThread( + this->dataPtr->renderThread); + + current->makeCurrent(this->window()); + + QMetaObject::invokeMethod(this, "Ready"); + return nullptr; + } + + if (!node) + { + node = new TextureNode(this->window()); + + // Set up connections to get the production of render texture in sync with + // vsync on the rendering thread. + // + // When a new texture is ready on the rendering thread, we use a direct + // connection to the texture node to let it know a new texture can be used. + // The node will then emit PendingNewTexture which we bind to + // QQuickWindow::update to schedule a redraw. + // + // When the scene graph starts rendering the next frame, the PrepareNode() + // function is used to update the node with the new texture. Once it + // completes, it emits TextureInUse() which we connect to the rendering + // thread's RenderNext() to have it start producing content into its render + // texture. + // + // This rendering pipeline is throttled by vsync on the scene graph + // rendering thread. + + this->connect(this->dataPtr->renderThread, &RenderThread::TextureReady, + node, &TextureNode::NewTexture, Qt::DirectConnection); + this->connect(node, &TextureNode::PendingNewTexture, this->window(), + &QQuickWindow::update, Qt::QueuedConnection); + this->connect(this->window(), &QQuickWindow::beforeRendering, node, + &TextureNode::PrepareNode, Qt::DirectConnection); + this->connect(node, &TextureNode::TextureInUse, this->dataPtr->renderThread, + &RenderThread::RenderNext, Qt::QueuedConnection); + + // Get the production of FBO textures started.. + QMetaObject::invokeMethod(this->dataPtr->renderThread, "RenderNext", + Qt::QueuedConnection); + } + + node->setRect(this->boundingRect()); + + return node; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetBackgroundColor(const math::Color &_color) +{ + this->dataPtr->renderThread->ignRenderer.backgroundColor = _color; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetAmbientLight(const math::Color &_ambient) +{ + this->dataPtr->renderThread->ignRenderer.ambientLight = _ambient; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetEngineName(const std::string &_name) +{ + this->dataPtr->renderThread->ignRenderer.engineName = _name; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetSceneName(const std::string &_name) +{ + this->dataPtr->renderThread->ignRenderer.sceneName = _name; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetCameraPose(const math::Pose3d &_pose) +{ + this->dataPtr->renderThread->ignRenderer.cameraPose = _pose; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetSceneService(const std::string &_service) +{ + this->dataPtr->renderThread->ignRenderer.sceneService = _service; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetPoseTopic(const std::string &_topic) +{ + this->dataPtr->renderThread->ignRenderer.poseTopic = _topic; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetDeletionTopic(const std::string &_topic) +{ + this->dataPtr->renderThread->ignRenderer.deletionTopic = _topic; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetSceneTopic(const std::string &_topic) +{ + this->dataPtr->renderThread->ignRenderer.sceneTopic = _topic; +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetSkyEnabled(const bool &_sky) +{ + this->dataPtr->renderThread->ignRenderer.skyEnable = _sky; +} + +///////////////////////////////////////////////// +MinimalScene::MinimalScene() + : Plugin(), dataPtr(new MinimalScenePrivate) +{ + qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); +} + + +///////////////////////////////////////////////// +MinimalScene::~MinimalScene() +{ +} + +///////////////////////////////////////////////// +void MinimalScene::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + RenderWindowItem *renderWindow = + this->PluginItem()->findChild(); + if (!renderWindow) + { + ignerr << "Unable to find Render Window item. " + << "Render window will not be created" << std::endl; + return; + } + + if (this->title.empty()) + this->title = "3D Scene"; + + // Custom parameters + if (_pluginElem) + { + auto elem = _pluginElem->FirstChildElement("engine"); + if (nullptr != elem && nullptr != elem->GetText()) + { + renderWindow->SetEngineName(elem->GetText()); + // there is a problem with displaying ogre2 render textures that are in + // sRGB format. Workaround for now is to apply gamma correction manually. + // There maybe a better way to solve the problem by making OpenGL calls.. + if (elem->GetText() == std::string("ogre2")) + this->PluginItem()->setProperty("gammaCorrect", true); + } + + elem = _pluginElem->FirstChildElement("scene"); + if (nullptr != elem && nullptr != elem->GetText()) + renderWindow->SetSceneName(elem->GetText()); + + elem = _pluginElem->FirstChildElement("ambient_light"); + if (nullptr != elem && nullptr != elem->GetText()) + { + math::Color ambient; + std::stringstream colorStr; + colorStr << std::string(elem->GetText()); + colorStr >> ambient; + renderWindow->SetAmbientLight(ambient); + } + + elem = _pluginElem->FirstChildElement("background_color"); + if (nullptr != elem && nullptr != elem->GetText()) + { + math::Color bgColor; + std::stringstream colorStr; + colorStr << std::string(elem->GetText()); + colorStr >> bgColor; + renderWindow->SetBackgroundColor(bgColor); + } + + elem = _pluginElem->FirstChildElement("camera_pose"); + if (nullptr != elem && nullptr != elem->GetText()) + { + math::Pose3d pose; + std::stringstream poseStr; + poseStr << std::string(elem->GetText()); + poseStr >> pose; + renderWindow->SetCameraPose(pose); + } + + elem = _pluginElem->FirstChildElement("service"); + if (nullptr != elem && nullptr != elem->GetText()) + { + std::string service = elem->GetText(); + renderWindow->SetSceneService(service); + } + + elem = _pluginElem->FirstChildElement("pose_topic"); + if (nullptr != elem && nullptr != elem->GetText()) + { + std::string topic = elem->GetText(); + renderWindow->SetPoseTopic(topic); + } + + elem = _pluginElem->FirstChildElement("deletion_topic"); + if (nullptr != elem && nullptr != elem->GetText()) + { + std::string topic = elem->GetText(); + renderWindow->SetDeletionTopic(topic); + } + + elem = _pluginElem->FirstChildElement("scene_topic"); + if (nullptr != elem && nullptr != elem->GetText()) + { + std::string topic = elem->GetText(); + renderWindow->SetSceneTopic(topic); + } + + elem = _pluginElem->FirstChildElement("sky"); + if (nullptr != elem && nullptr != elem->GetText()) + { + renderWindow->SetSkyEnabled(true); + if (!elem->NoChildren()) + ignwarn << "Child elements of are not supported yet" << std::endl; + } + } +} + +///////////////////////////////////////////////// +void RenderWindowItem::mousePressEvent(QMouseEvent *_e) +{ + auto event = convert(*_e); + event.SetPressPos(event.Pos()); + this->dataPtr->mouseEvent = event; + + this->dataPtr->renderThread->ignRenderer.NewMouseEvent( + this->dataPtr->mouseEvent); +} + +//////////////////////////////////////////////// +void RenderWindowItem::mouseReleaseEvent(QMouseEvent *_e) +{ + this->dataPtr->mouseEvent = convert(*_e); + + this->dataPtr->renderThread->ignRenderer.NewMouseEvent( + this->dataPtr->mouseEvent); +} + +//////////////////////////////////////////////// +void RenderWindowItem::mouseMoveEvent(QMouseEvent *_e) +{ + auto event = convert(*_e); + event.SetPressPos(this->dataPtr->mouseEvent.PressPos()); + + if (!event.Dragging()) + return; + + auto dragInt = event.Pos() - this->dataPtr->mouseEvent.Pos(); + auto dragDistance = math::Vector2d(dragInt.X(), dragInt.Y()); + + this->dataPtr->renderThread->ignRenderer.NewMouseEvent(event, dragDistance); + this->dataPtr->mouseEvent = event; +} + +//////////////////////////////////////////////// +void RenderWindowItem::wheelEvent(QWheelEvent *_e) +{ + this->dataPtr->mouseEvent.SetType(common::MouseEvent::SCROLL); +#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) + this->dataPtr->mouseEvent.SetPos(_e->x(), _e->y()); +#else + this->dataPtr->mouseEvent.SetPos(_e->position().x(), _e->position().y()); +#endif + double scroll = (_e->angleDelta().y() > 0) ? -1.0 : 1.0; + this->dataPtr->renderThread->ignRenderer.NewMouseEvent( + this->dataPtr->mouseEvent, math::Vector2d(scroll, scroll)); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gui::plugins::MinimalScene, + ignition::gui::Plugin) diff --git a/src/plugins/minimal_scene/MinimalScene.hh b/src/plugins/minimal_scene/MinimalScene.hh new file mode 100644 index 000000000..2319e0869 --- /dev/null +++ b/src/plugins/minimal_scene/MinimalScene.hh @@ -0,0 +1,342 @@ +/* + * Copyright (C) 2017 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_GUI_PLUGINS_SCENE3D_HH_ +#define IGNITION_GUI_PLUGINS_SCENE3D_HH_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "ignition/gui/qt.h" +#include "ignition/gui/Plugin.hh" + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + class IgnRendererPrivate; + class RenderWindowItemPrivate; + class MinimalScenePrivate; + + /// \brief Creates a new ignition rendering scene or adds a user-camera to an + /// existing scene. It is possible to orbit the camera around the scene with + /// the mouse. Use other plugins to manage objects in the scene. + /// + /// ## Configuration + /// + /// * \ : Optional render engine name, defaults to 'ogre'. + /// * \ : Optional scene name, defaults to 'scene'. The plugin will + /// create a scene with this name if there isn't one yet. If + /// there is already one, a new camera is added to it. + /// * \ : Optional color for ambient light, defaults to + /// (0.3, 0.3, 0.3, 1.0) + /// * \ : Optional background color, defaults to + /// (0.3, 0.3, 0.3, 1.0) + /// * \ : Optional starting pose for the camera, defaults to + /// (0, 0, 5, 0, 0, 0) + class MinimalScene : public Plugin + { + Q_OBJECT + + /// \brief Constructor + public: MinimalScene(); + + /// \brief Destructor + public: virtual ~MinimalScene(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; + + /// \brief Ign-rendering renderer. + /// All ign-rendering calls should be performed inside this class as it makes + /// sure that opengl calls in the underlying render engine do not interfere + /// with QtQuick's opengl render operations. The main Render function will + /// render to an offscreen texture and notify via signal and slots when it's + /// ready to be displayed. + class IgnRenderer + { + /// \brief Constructor + public: IgnRenderer(); + + /// \brief Destructor + public: ~IgnRenderer(); + + /// \brief Main render function + public: void Render(); + + /// \brief Initialize the render engine + public: void Initialize(); + + /// \brief Destroy camera associated with this renderer + public: void Destroy(); + + /// \brief New mouse event triggered + /// \param[in] _e New mouse event + /// \param[in] _drag Mouse move distance + public: void NewMouseEvent(const common::MouseEvent &_e, + const math::Vector2d &_drag = math::Vector2d::Zero); + + /// \brief Handle mouse event for view control + private: void HandleMouseEvent(); + + /// \brief Retrieve the first point on a surface in the 3D scene hit by a + /// ray cast from the given 2D screen coordinates. + /// \param[in] _screenPos 2D coordinates on the screen, in pixels. + /// \return 3D coordinates of a point in the 3D scene. + private: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos) + const; + + /// \brief Render texture id + public: GLuint textureId = 0u; + + /// \brief Render engine to use + public: std::string engineName = "ogre"; + + /// \brief Unique scene name + public: std::string sceneName = "scene"; + + /// \brief Initial Camera pose + public: math::Pose3d cameraPose = math::Pose3d(0, 0, 2, 0, 0.4, 0); + + /// \brief Scene background color + public: math::Color backgroundColor = math::Color::Black; + + /// \brief Ambient color + public: math::Color ambientLight = math::Color(0.3f, 0.3f, 0.3f, 1.0f); + + /// \brief True if engine has been initialized; + public: bool initialized = false; + + /// \brief Render texture size + public: QSize textureSize = QSize(1024, 1024); + + /// \brief Flag to indicate texture size has changed. + public: bool textureDirty = false; + + /// \brief Scene service. If not empty, a request will be made to get the + /// scene information using this service and the renderer will populate the + /// scene based on the response data + public: std::string sceneService; + + /// \brief Scene pose topic. If not empty, a node will subcribe to this + /// topic to get pose updates of objects in the scene + public: std::string poseTopic; + + /// \brief Ign-transport deletion topic name + public: std::string deletionTopic; + + /// \brief Ign-transport scene topic name + /// New scene messages will be published to this topic when an entities are + /// added + public: std::string sceneTopic; + + /// \brief True if sky is enabled; + public: bool skyEnable = false; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; + + /// \brief Rendering thread + class RenderThread : public QThread + { + Q_OBJECT + + /// \brief Constructor + public: RenderThread(); + + /// \brief Render the next frame + public slots: void RenderNext(); + + /// \brief Shutdown the thread and the render engine + public slots: void ShutDown(); + + /// \brief Slot called to update render texture size + public slots: void SizeChanged(); + + /// \brief Signal to indicate that a frame has been rendered and ready + /// to be displayed + /// \param[in] _id GLuid of the opengl texture + /// \param[in] _size Size of the texture + signals: void TextureReady(int _id, const QSize &_size); + + /// \brief Offscreen surface to render to + public: QOffscreenSurface *surface = nullptr; + + /// \brief OpenGL context to be passed to the render engine + public: QOpenGLContext *context = nullptr; + + /// \brief Ign-rendering renderer + public: IgnRenderer ignRenderer; + }; + + + /// \brief A QQUickItem that manages the render window + class RenderWindowItem : public QQuickItem + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _parent Parent item + public: explicit RenderWindowItem(QQuickItem *_parent = nullptr); + + /// \brief Destructor + public: virtual ~RenderWindowItem(); + + /// \brief Set background color of render window + /// \param[in] _color Color of render window background + public: void SetBackgroundColor(const math::Color &_color); + + /// \brief Set ambient light of render window + /// \param[in] _ambient Color of ambient light + public: void SetAmbientLight(const math::Color &_ambient); + + /// \brief Set engine name used to create the render window + /// \param[in] _name Name of render engine + public: void SetEngineName(const std::string &_name); + + /// \brief Set name of scene created inside the render window + /// \param[in] _name Name of scene + public: void SetSceneName(const std::string &_name); + + /// \brief Set the initial pose the render window camera + /// \param[in] _pose Initical camera pose + public: void SetCameraPose(const math::Pose3d &_pose); + + /// \brief Set scene service to use in this render window + /// A service call will be made using ign-transport to get scene + /// data using this service + /// \param[in] _service Scene service name + public: void SetSceneService(const std::string &_service); + + /// \brief Set pose topic to use for updating objects in the scene + /// The renderer will subscribe to this topic to get pose messages of + /// visuals in the scene + /// \param[in] _topic Pose topic + public: void SetPoseTopic(const std::string &_topic); + + /// \brief Set deletion topic to use for deleting objects from the scene + /// The renderer will subscribe to this topic to get notified when entities + /// in the scene get deleted + /// \param[in] _topic Deletion topic + public: void SetDeletionTopic(const std::string &_topic); + + /// \brief Set the scene topic to use for updating objects in the scene + /// The renderer will subscribe to this topic to get updates scene messages + /// \param[in] _topic Scene topic + public: void SetSceneTopic(const std::string &_topic); + + /// \brief Set if sky is enabled + /// \param[in] _sky True to enable the sky, false otherwise. + public: void SetSkyEnabled(const bool &_sky); + + /// \brief Slot called when thread is ready to be started + public Q_SLOTS: void Ready(); + + // Documentation inherited + protected: virtual void mousePressEvent(QMouseEvent *_e) override; + + // Documentation inherited + protected: virtual void mouseReleaseEvent(QMouseEvent *_e) override; + + // Documentation inherited + protected: virtual void mouseMoveEvent(QMouseEvent *_e) override; + + // Documentation inherited + protected: virtual void wheelEvent(QWheelEvent *_e) override; + + /// \brief Overrides the paint event to render the render engine + /// camera view + /// \param[in] _oldNode The node passed in previous updatePaintNode + /// function. It represents the visual representation of the item. + /// \param[in] _data The node transformation data. + /// \return Updated node. + private: QSGNode *updatePaintNode(QSGNode *_oldNode, + QQuickItem::UpdatePaintNodeData *_data) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; + + /// \brief Texture node for displaying the render texture from ign-renderer + class TextureNode : public QObject, public QSGSimpleTextureNode + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _window Parent window + public: explicit TextureNode(QQuickWindow *_window); + + /// \brief Destructor + public: ~TextureNode() override; + + /// \brief This function gets called on the FBO rendering thread and will + /// store the texture id and size and schedule an update on the window. + /// \param[in] _id OpenGL render texture Id + /// \param[in] _size Texture size + public slots: void NewTexture(int _id, const QSize &_size); + + /// \brief Before the scene graph starts to render, we update to the + /// pending texture + public slots: void PrepareNode(); + + /// \brief Signal emitted when the texture is being rendered and renderer + /// can start rendering next frame + signals: void TextureInUse(); + + /// \brief Signal emitted when a new texture is ready to trigger window + /// update + signals: void PendingNewTexture(); + + /// \brief OpenGL texture id + public: int id = 0; + + /// \brief Texture size + public: QSize size = QSize(0, 0); + + /// \brief Mutex to protect the texture variables + public: QMutex mutex; + + /// \brief Qt's scene graph texture + public: QSGTexture *texture = nullptr; + + /// \brief Qt quick window + public: QQuickWindow *window = nullptr; + }; +} +} +} + +#endif diff --git a/src/plugins/minimal_scene/MinimalScene.qml b/src/plugins/minimal_scene/MinimalScene.qml new file mode 100644 index 000000000..304ccf186 --- /dev/null +++ b/src/plugins/minimal_scene/MinimalScene.qml @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2018 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.0 +import QtQuick.Controls 2.0 +import RenderWindow 1.0 +import QtGraphicalEffects 1.0 + +Rectangle { + width: 1000 + height: 800 + + /** + * True to enable gamma correction + */ + property bool gammaCorrect: false + + + RenderWindow { + id: renderWindow + objectName: "rw" + anchors.fill: parent + } + + /* + * Gamma correction for sRGB output. Enabled when engine is set to ogre2 + */ + GammaAdjust { + anchors.fill: renderWindow + source: renderWindow + gamma: 2.4 + enabled: gammaCorrect + visible: gammaCorrect + } + + onParentChanged: { + if (undefined === parent) + return; + + width = Qt.binding(function() {return parent.parent.width}) + height = Qt.binding(function() {return parent.parent.height}) + } +} diff --git a/src/plugins/minimal_scene/MinimalScene.qrc b/src/plugins/minimal_scene/MinimalScene.qrc new file mode 100644 index 000000000..3055e40cb --- /dev/null +++ b/src/plugins/minimal_scene/MinimalScene.qrc @@ -0,0 +1,5 @@ + + + MinimalScene.qml + + diff --git a/src/plugins/minimal_scene/MinimalScene_TEST.cc b/src/plugins/minimal_scene/MinimalScene_TEST.cc new file mode 100644 index 000000000..0c798904a --- /dev/null +++ b/src/plugins/minimal_scene/MinimalScene_TEST.cc @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2017 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 "ignition/gui/Iface.hh" +#include "ignition/gui/MainWindow.hh" +#include "ignition/gui/Plugin.hh" + +using namespace ignition; +using namespace gui; + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, Load) +{ + setVerbosity(4); + EXPECT_TRUE(initApp()); + + EXPECT_TRUE(loadPlugin("MinimalScene")); + + EXPECT_TRUE(stop()); +} + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, Resize) +{ + setVerbosity(4); + EXPECT_TRUE(initApp()); + + // Load plugin + EXPECT_TRUE(loadPlugin("MinimalScene")); + + // Create main window + EXPECT_TRUE(createMainWindow()); + + // Close window after some time + auto win = mainWindow(); + ASSERT_NE(nullptr, win); + + QTimer::singleShot(300, [&win]() + { + // Check there are no segfaults when resizing + for (auto i : {100, 300, 200, 500, 400}) + { + win->resize(i + (qrand() % 100), i + (qrand() % 100)); + QCoreApplication::processEvents(); + } + win->close(); + }); + + // Show window + EXPECT_TRUE(runMainWindow()); + + EXPECT_TRUE(stop()); +} + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, Config) +{ + setVerbosity(4); + EXPECT_TRUE(initApp()); + + // Load plugin + const char *pluginStr = + "" + "ogre" + "banana" + "1.0 0 0" + "0 1 0" + "1 2 3 0 0 1.57" + ""; + + tinyxml2::XMLDocument pluginDoc; + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(ignition::gui::loadPlugin("MinimalScene", + pluginDoc.FirstChildElement("plugin"))); + + // Create main window + EXPECT_TRUE(createMainWindow()); + + // Check scene + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + auto scene = engine->SceneByName("banana"); + ASSERT_NE(nullptr, scene); + + EXPECT_EQ(math::Color(0, 1, 0), scene->BackgroundColor()); + EXPECT_EQ(math::Color(1, 0, 0), scene->AmbientLight()); + + auto root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + EXPECT_EQ(1u, root->ChildCount()); + + // Check camera + auto camera = std::dynamic_pointer_cast( + root->ChildByIndex(0)); + ASSERT_NE(nullptr, camera); + + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1.57), camera->WorldPose()); + + EXPECT_TRUE(stop()); +} + diff --git a/src/plugins/scene3d/CMakeLists.txt b/src/plugins/scene3d/CMakeLists.txt index 0ba0a7b2a..469226031 100644 --- a/src/plugins/scene3d/CMakeLists.txt +++ b/src/plugins/scene3d/CMakeLists.txt @@ -6,6 +6,6 @@ ign_gui_add_plugin(Scene3D TEST_SOURCES # Scene3D_TEST.cc PUBLIC_LINK_LIBS - ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ${IGNITION-RENDERING_LIBRARIES} ) diff --git a/src/plugins/scene3d/Scene3D.cc b/src/plugins/scene3d/Scene3D.cc index e325dd7a5..da210cd4b 100644 --- a/src/plugins/scene3d/Scene3D.cc +++ b/src/plugins/scene3d/Scene3D.cc @@ -32,11 +32,13 @@ #include #include -// TODO(louise) Remove these pragmas once ign-rendering -// is disabling the warnings +// TODO(louise) Remove these pragmas once ign-rendering and ign-msgs +// are disabling the warnings #ifdef _MSC_VER #pragma warning(push, 0) #endif +#include + #include #include #include @@ -48,6 +50,8 @@ #pragma warning(pop) #endif +#include + #include "ignition/gui/Application.hh" #include "ignition/gui/Conversions.hh" #include "ignition/gui/GuiEvents.hh" @@ -59,6 +63,144 @@ namespace gui { namespace plugins { + /// \brief Scene manager class for loading and managing objects in the scene + class SceneManager + { + /// \brief Constructor + public: SceneManager(); + + /// \brief Constructor + /// \param[in] _service Ign transport scene service name + /// \param[in] _poseTopic Ign transport pose topic name + /// \param[in] _deletionTopic Ign transport deletion topic name + /// \param[in] _sceneTopic Ign transport scene topic name + /// \param[in] _scene Pointer to the rendering scene + public: SceneManager(const std::string &_service, + const std::string &_poseTopic, + const std::string &_deletionTopic, + const std::string &_sceneTopic, + rendering::ScenePtr _scene); + + /// \brief Load the scene manager + /// \param[in] _service Ign transport service name + /// \param[in] _poseTopic Ign transport pose topic name + /// \param[in] _deletionTopic Ign transport deletion topic name + /// \param[in] _sceneTopic Ign transport scene topic name + /// \param[in] _scene Pointer to the rendering scene + public: void Load(const std::string &_service, + const std::string &_poseTopic, + const std::string &_deletionTopic, + const std::string &_sceneTopic, + rendering::ScenePtr _scene); + + /// \brief Make the scene service request and populate the scene + public: void Request(); + + /// \brief Update the scene based on pose msgs received + public: void Update(); + + /// \brief Callback function for the pose topic + /// \param[in] _msg Pose vector msg + private: void OnPoseVMsg(const msgs::Pose_V &_msg); + + /// \brief Load the scene from a scene msg + /// \param[in] _msg Scene msg + private: void LoadScene(const msgs::Scene &_msg); + + /// \brief Callback function for the request topic + /// \param[in] _msg Deletion message + private: void OnDeletionMsg(const msgs::UInt32_V &_msg); + + /// \brief Load the scene from a scene msg + /// \param[in] _msg Scene msg + private: void OnSceneSrvMsg(const msgs::Scene &_msg, const bool result); + + /// \brief Called when there's an entity is added to the scene + /// \param[in] _msg Scene msg + private: void OnSceneMsg(const msgs::Scene &_msg); + + /// \brief Load the model from a model msg + /// \param[in] _msg Model msg + /// \return Model visual created from the msg + private: rendering::VisualPtr LoadModel(const msgs::Model &_msg); + + /// \brief Load a link from a link msg + /// \param[in] _msg Link msg + /// \return Link visual created from the msg + private: rendering::VisualPtr LoadLink(const msgs::Link &_msg); + + /// \brief Load a visual from a visual msg + /// \param[in] _msg Visual msg + /// \return Visual visual created from the msg + private: rendering::VisualPtr LoadVisual(const msgs::Visual &_msg); + + /// \brief Load a geometry from a geometry msg + /// \param[in] _msg Geometry msg + /// \param[out] _scale Geometry scale that will be set based on msg param + /// \param[out] _localPose Additional local pose to be applied after the + /// visual's pose + /// \return Geometry object created from the msg + private: rendering::GeometryPtr LoadGeometry(const msgs::Geometry &_msg, + math::Vector3d &_scale, math::Pose3d &_localPose); + + /// \brief Load a material from a material msg + /// \param[in] _msg Material msg + /// \return Material object created from the msg + private: rendering::MaterialPtr LoadMaterial(const msgs::Material &_msg); + + /// \brief Load a light from a light msg + /// \param[in] _msg Light msg + /// \return Light object created from the msg + private: rendering::LightPtr LoadLight(const msgs::Light &_msg); + + /// \brief Delete an entity + /// \param[in] _entity Entity to delete + private: void DeleteEntity(const unsigned int _entity); + + //// \brief Ign-transport scene service name + private: std::string service; + + //// \brief Ign-transport pose topic name + private: std::string poseTopic; + + //// \brief Ign-transport deletion topic name + private: std::string deletionTopic; + + //// \brief Ign-transport scene topic name + private: std::string sceneTopic; + + //// \brief Pointer to the rendering scene + private: rendering::ScenePtr scene; + + //// \brief Mutex to protect the pose msgs + private: std::mutex mutex; + + /// \brief Map of entity id to pose + private: std::map poses; + + /// \brief Map of entity id to initial local poses + /// This is currently used to handle the normal vector in plane visuals. In + /// general, this can be used to store any local transforms between the + /// parent Visual and geometry. + private: std::map localPoses; + + /// \brief Map of visual id to visual pointers. + private: std::map visuals; + + /// \brief Map of light id to light pointers. + private: std::map lights; + + /// Entities to be deleted + private: std::vector toDeleteEntities; + + /// \brief Keeps the a list of unprocessed scene messages + private: std::vector sceneMsgs; + + /// \brief Transport node for making service request and subscribing to + /// pose topic + private: ignition::transport::Node node; + }; + /// \brief Private data class for IgnRenderer class IgnRendererPrivate { @@ -83,6 +225,9 @@ namespace plugins /// \brief Ray query for mouse clicks public: rendering::RayQueryPtr rayQuery; + /// \brief Scene requester to get scene info + public: SceneManager sceneManager; + /// \brief View control focus target public: math::Vector3d target; }; @@ -114,6 +259,584 @@ using namespace plugins; QList RenderWindowItemPrivate::threads; +///////////////////////////////////////////////// +SceneManager::SceneManager() +{ +} + +///////////////////////////////////////////////// +SceneManager::SceneManager(const std::string &_service, + const std::string &_poseTopic, + const std::string &_deletionTopic, + const std::string &_sceneTopic, + rendering::ScenePtr _scene) +{ + this->Load(_service, _poseTopic, _deletionTopic, _sceneTopic, _scene); +} + +///////////////////////////////////////////////// +void SceneManager::Load(const std::string &_service, + const std::string &_poseTopic, + const std::string &_deletionTopic, + const std::string &_sceneTopic, + rendering::ScenePtr _scene) +{ + this->service = _service; + this->poseTopic = _poseTopic; + this->deletionTopic = _deletionTopic; + this->sceneTopic = _sceneTopic; + this->scene = _scene; +} + +///////////////////////////////////////////////// +void SceneManager::Request() +{ + // wait for the service to be advertized + std::vector publishers; + const std::chrono::duration sleepDuration{1.0}; + const std::size_t tries = 30; + for (std::size_t i = 0; i < tries; ++i) + { + this->node.ServiceInfo(this->service, publishers); + if (publishers.size() > 0) + break; + std::this_thread::sleep_for(sleepDuration); + igndbg << "Waiting for service " << this->service << "\n"; + } + + if (publishers.empty() || + !this->node.Request(this->service, &SceneManager::OnSceneSrvMsg, this)) + { + ignerr << "Error making service request to " << this->service << std::endl; + } +} + +///////////////////////////////////////////////// +void SceneManager::OnPoseVMsg(const msgs::Pose_V &_msg) +{ + std::lock_guard lock(this->mutex); + for (int i = 0; i < _msg.pose_size(); ++i) + { + math::Pose3d pose = msgs::Convert(_msg.pose(i)); + + // apply additional local poses if available + const auto it = this->localPoses.find(_msg.pose(i).id()); + if (it != this->localPoses.end()) + { + pose = pose * it->second; + } + + this->poses[_msg.pose(i).id()] = pose; + } +} + +///////////////////////////////////////////////// +void SceneManager::OnDeletionMsg(const msgs::UInt32_V &_msg) +{ + std::lock_guard lock(this->mutex); + std::copy(_msg.data().begin(), _msg.data().end(), + std::back_inserter(this->toDeleteEntities)); +} + +///////////////////////////////////////////////// +void SceneManager::Update() +{ + // process msgs + std::lock_guard lock(this->mutex); + + for (const auto &msg : this->sceneMsgs) + { + this->LoadScene(msg); + } + this->sceneMsgs.clear(); + + for (const auto &entity : this->toDeleteEntities) + { + this->DeleteEntity(entity); + } + this->toDeleteEntities.clear(); + + + for (auto pIt = this->poses.begin(); pIt != this->poses.end();) + { + auto vIt = this->visuals.find(pIt->first); + if (vIt != this->visuals.end()) + { + auto visual = vIt->second.lock(); + if (visual) + { + visual->SetLocalPose(pIt->second); + } + else + { + this->visuals.erase(vIt); + } + this->poses.erase(pIt++); + } + else + { + auto lIt = this->lights.find(pIt->first); + if (lIt != this->lights.end()) + { + auto light = lIt->second.lock(); + if (light) + { + light->SetLocalPose(pIt->second); + } + else + { + this->lights.erase(lIt); + } + this->poses.erase(pIt++); + } + else + { + ++pIt; + } + } + } + + // Note we are clearing the pose msgs here but later on we may need to + // consider the case where pose msgs arrive before scene/visual msgs + this->poses.clear(); +} + + +///////////////////////////////////////////////// +void SceneManager::OnSceneMsg(const msgs::Scene &_msg) +{ + std::lock_guard lock(this->mutex); + this->sceneMsgs.push_back(_msg); +} + +///////////////////////////////////////////////// +void SceneManager::OnSceneSrvMsg(const msgs::Scene &_msg, const bool result) +{ + if (!result) + { + ignerr << "Error making service request to " << this->service + << std::endl; + return; + } + + { + std::lock_guard lock(this->mutex); + this->sceneMsgs.push_back(_msg); + } + + if (!this->poseTopic.empty()) + { + if (!this->node.Subscribe(this->poseTopic, &SceneManager::OnPoseVMsg, this)) + { + ignerr << "Error subscribing to pose topic: " << this->poseTopic + << std::endl; + } + } + else + { + ignwarn << "The pose topic, set via , for the Scene3D plugin " + << "is missing or empty. Please set this topic so that the Scene3D " + << "can receive and process pose information.\n"; + } + + if (!this->deletionTopic.empty()) + { + if (!this->node.Subscribe(this->deletionTopic, &SceneManager::OnDeletionMsg, + this)) + { + ignerr << "Error subscribing to deletion topic: " << this->deletionTopic + << std::endl; + } + } + else + { + ignwarn << "The deletion topic, set via , for the " + << "Scene3D plugin is missing or empty. Please set this topic so that " + << "the Scene3D can receive and process deletion information.\n"; + } + + if (!this->sceneTopic.empty()) + { + if (!this->node.Subscribe( + this->sceneTopic, &SceneManager::OnSceneMsg, this)) + { + ignerr << "Error subscribing to scene topic: " << this->sceneTopic + << std::endl; + } + } + else + { + ignwarn << "The scene topic, set via , for the " + << "Scene3D plugin is missing or empty. Please set this topic so that " + << "the Scene3D can receive and process scene information.\n"; + } +} + +void SceneManager::LoadScene(const msgs::Scene &_msg) +{ + rendering::VisualPtr rootVis = this->scene->RootVisual(); + + // load models + for (int i = 0; i < _msg.model_size(); ++i) + { + // Only add if it's not already loaded + if (this->visuals.find(_msg.model(i).id()) == this->visuals.end()) + { + rendering::VisualPtr modelVis = this->LoadModel(_msg.model(i)); + if (modelVis) + rootVis->AddChild(modelVis); + else + ignerr << "Failed to load model: " << _msg.model(i).name() << std::endl; + } + } + + // load lights + for (int i = 0; i < _msg.light_size(); ++i) + { + if (this->lights.find(_msg.light(i).id()) == this->lights.end()) + { + rendering::LightPtr light = this->LoadLight(_msg.light(i)); + if (light) + rootVis->AddChild(light); + else + ignerr << "Failed to load light: " << _msg.light(i).name() << std::endl; + } + } +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::LoadModel(const msgs::Model &_msg) +{ + rendering::VisualPtr modelVis = this->scene->CreateVisual(); + if (_msg.has_pose()) + modelVis->SetLocalPose(msgs::Convert(_msg.pose())); + this->visuals[_msg.id()] = modelVis; + + // load links + for (int i = 0; i < _msg.link_size(); ++i) + { + rendering::VisualPtr linkVis = this->LoadLink(_msg.link(i)); + if (linkVis) + modelVis->AddChild(linkVis); + else + ignerr << "Failed to load link: " << _msg.link(i).name() << std::endl; + } + + // load nested models + for (int i = 0; i < _msg.model_size(); ++i) + { + rendering::VisualPtr nestedModelVis = this->LoadModel(_msg.model(i)); + if (nestedModelVis) + modelVis->AddChild(nestedModelVis); + else + ignerr << "Failed to load nested model: " << _msg.model(i).name() + << std::endl; + } + + return modelVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::LoadLink(const msgs::Link &_msg) +{ + rendering::VisualPtr linkVis = this->scene->CreateVisual(); + if (_msg.has_pose()) + linkVis->SetLocalPose(msgs::Convert(_msg.pose())); + this->visuals[_msg.id()] = linkVis; + + // load visuals + for (int i = 0; i < _msg.visual_size(); ++i) + { + rendering::VisualPtr visualVis = this->LoadVisual(_msg.visual(i)); + if (visualVis) + linkVis->AddChild(visualVis); + else + ignerr << "Failed to load visual: " << _msg.visual(i).name() << std::endl; + } + + // load lights + for (int i = 0; i < _msg.light_size(); ++i) + { + rendering::LightPtr light = this->LoadLight(_msg.light(i)); + if (light) + linkVis->AddChild(light); + else + ignerr << "Failed to load light: " << _msg.light(i).name() << std::endl; + } + + return linkVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::LoadVisual(const msgs::Visual &_msg) +{ + if (!_msg.has_geometry()) + return rendering::VisualPtr(); + + rendering::VisualPtr visualVis = this->scene->CreateVisual(); + this->visuals[_msg.id()] = visualVis; + + math::Vector3d scale = math::Vector3d::One; + math::Pose3d localPose; + rendering::GeometryPtr geom = + this->LoadGeometry(_msg.geometry(), scale, localPose); + + if (_msg.has_pose()) + visualVis->SetLocalPose(msgs::Convert(_msg.pose()) * localPose); + else + visualVis->SetLocalPose(localPose); + + if (geom) + { + // store the local pose + this->localPoses[_msg.id()] = localPose; + + visualVis->AddGeometry(geom); + visualVis->SetLocalScale(scale); + + // set material + rendering::MaterialPtr material{nullptr}; + if (_msg.has_material()) + { + material = this->LoadMaterial(_msg.material()); + } + // Don't set a default material for meshes because they + // may have their own + // TODO(anyone) support overriding mesh material + else if (!_msg.geometry().has_mesh()) + { + // create default material + material = this->scene->Material("ign-grey"); + if (!material) + { + material = this->scene->CreateMaterial("ign-grey"); + material->SetAmbient(0.3, 0.3, 0.3); + material->SetDiffuse(0.7, 0.7, 0.7); + material->SetSpecular(1.0, 1.0, 1.0); + material->SetRoughness(0.2f); + material->SetMetalness(1.0f); + } + } + else + { + // meshes created by mesh loader may have their own materials + // update/override their properties based on input sdf element values + auto mesh = std::dynamic_pointer_cast(geom); + for (unsigned int i = 0; i < mesh->SubMeshCount(); ++i) + { + auto submesh = mesh->SubMeshByIndex(i); + auto submeshMat = submesh->Material(); + if (submeshMat) + { + double productAlpha = (1.0-_msg.transparency()) * + (1.0 - submeshMat->Transparency()); + submeshMat->SetTransparency(1 - productAlpha); + submeshMat->SetCastShadows(_msg.cast_shadows()); + } + } + } + + if (material) + { + // set transparency + material->SetTransparency(_msg.transparency()); + + // cast shadows + material->SetCastShadows(_msg.cast_shadows()); + + geom->SetMaterial(material); + // todo(anyone) SetMaterial function clones the input material. + // but does not take ownership of it so we need to destroy it here. + // This is not ideal. We should let ign-rendering handle the lifetime + // of this material + this->scene->DestroyMaterial(material); + } + } + else + { + ignerr << "Failed to load geometry for visual: " << _msg.name() + << std::endl; + } + + return visualVis; +} + +///////////////////////////////////////////////// +rendering::GeometryPtr SceneManager::LoadGeometry(const msgs::Geometry &_msg, + math::Vector3d &_scale, math::Pose3d &_localPose) +{ + math::Vector3d scale = math::Vector3d::One; + math::Pose3d localPose = math::Pose3d::Zero; + rendering::GeometryPtr geom{nullptr}; + if (_msg.has_box()) + { + geom = this->scene->CreateBox(); + if (_msg.box().has_size()) + scale = msgs::Convert(_msg.box().size()); + } + else if (_msg.has_cylinder()) + { + geom = this->scene->CreateCylinder(); + scale.X() = _msg.cylinder().radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _msg.cylinder().length(); + } + else if (_msg.has_plane()) + { + geom = this->scene->CreatePlane(); + + if (_msg.plane().has_size()) + { + scale.X() = _msg.plane().size().x(); + scale.Y() = _msg.plane().size().y(); + } + + if (_msg.plane().has_normal()) + { + // Create a rotation for the plane mesh to account for the normal vector. + // The rotation is the angle between the +z(0,0,1) vector and the + // normal, which are both expressed in the local (Visual) frame. + math::Vector3d normal = msgs::Convert(_msg.plane().normal()); + localPose.Rot().From2Axes(math::Vector3d::UnitZ, normal.Normalized()); + } + } + else if (_msg.has_sphere()) + { + geom = this->scene->CreateSphere(); + scale.X() = _msg.sphere().radius() * 2; + scale.Y() = scale.X(); + scale.Z() = scale.X(); + } + else if (_msg.has_mesh()) + { + if (_msg.mesh().filename().empty()) + { + ignerr << "Mesh geometry missing filename" << std::endl; + return geom; + } + rendering::MeshDescriptor descriptor; + + // Assume absolute path to mesh file + descriptor.meshName = _msg.mesh().filename(); + + ignition::common::MeshManager* meshManager = + ignition::common::MeshManager::Instance(); + descriptor.mesh = meshManager->Load(descriptor.meshName); + geom = this->scene->CreateMesh(descriptor); + + scale = msgs::Convert(_msg.mesh().scale()); + } + else + { + ignerr << "Unsupported geometry type" << std::endl; + } + _scale = scale; + _localPose = localPose; + return geom; +} + +///////////////////////////////////////////////// +rendering::MaterialPtr SceneManager::LoadMaterial(const msgs::Material &_msg) +{ + rendering::MaterialPtr material = this->scene->CreateMaterial(); + if (_msg.has_ambient()) + { + material->SetAmbient(msgs::Convert(_msg.ambient())); + } + if (_msg.has_diffuse()) + { + material->SetDiffuse(msgs::Convert(_msg.diffuse())); + } + if (_msg.has_specular()) + { + material->SetDiffuse(msgs::Convert(_msg.specular())); + } + if (_msg.has_emissive()) + { + material->SetEmissive(msgs::Convert(_msg.emissive())); + } + + return material; +} + +///////////////////////////////////////////////// +rendering::LightPtr SceneManager::LoadLight(const msgs::Light &_msg) +{ + rendering::LightPtr light; + + switch (_msg.type()) + { + case msgs::Light_LightType_POINT: + light = this->scene->CreatePointLight(); + break; + case msgs::Light_LightType_SPOT: + { + light = this->scene->CreateSpotLight(); + rendering::SpotLightPtr spotLight = + std::dynamic_pointer_cast(light); + spotLight->SetInnerAngle(_msg.spot_inner_angle()); + spotLight->SetOuterAngle(_msg.spot_outer_angle()); + spotLight->SetFalloff(_msg.spot_falloff()); + break; + } + case msgs::Light_LightType_DIRECTIONAL: + { + light = this->scene->CreateDirectionalLight(); + rendering::DirectionalLightPtr dirLight = + std::dynamic_pointer_cast(light); + + if (_msg.has_direction()) + dirLight->SetDirection(msgs::Convert(_msg.direction())); + break; + } + default: + ignerr << "Light type not supported" << std::endl; + return light; + } + + if (_msg.has_pose()) + light->SetLocalPose(msgs::Convert(_msg.pose())); + + if (_msg.has_diffuse()) + light->SetDiffuseColor(msgs::Convert(_msg.diffuse())); + + if (_msg.has_specular()) + light->SetSpecularColor(msgs::Convert(_msg.specular())); + + light->SetAttenuationConstant(_msg.attenuation_constant()); + light->SetAttenuationLinear(_msg.attenuation_linear()); + light->SetAttenuationQuadratic(_msg.attenuation_quadratic()); + light->SetAttenuationRange(_msg.range()); + + light->SetCastShadows(_msg.cast_shadows()); + + this->lights[_msg.id()] = light; + return light; +} + +///////////////////////////////////////////////// +void SceneManager::DeleteEntity(const unsigned int _entity) +{ + if (this->visuals.find(_entity) != this->visuals.end()) + { + auto visual = this->visuals[_entity].lock(); + if (visual) + { + this->scene->DestroyVisual(visual, true); + } + this->visuals.erase(_entity); + } + else if (this->lights.find(_entity) != this->lights.end()) + { + auto light = this->lights[_entity].lock(); + if (light) + { + this->scene->DestroyLight(light, true); + } + this->lights.erase(_entity); + } +} + ///////////////////////////////////////////////// IgnRenderer::IgnRenderer() : dataPtr(new IgnRendererPrivate) @@ -137,10 +860,12 @@ void IgnRenderer::Render() this->textureSize.height()); // setting the size should cause the render texture to be rebuilt this->dataPtr->camera->PreRender(); + this->textureId = this->dataPtr->camera->RenderTextureGLId(); this->textureDirty = false; } - this->textureId = this->dataPtr->camera->RenderTextureGLId(); + // update the scene + this->dataPtr->sceneManager.Update(); // view control this->HandleMouseEvent(); @@ -240,11 +965,6 @@ void IgnRenderer::Initialize() scene->SetBackgroundColor(this->backgroundColor); } - if (this->skyEnable) - { - scene->SetSkyEnabled(true); - } - auto root = scene->RootVisual(); // Camera @@ -260,6 +980,15 @@ void IgnRenderer::Initialize() this->dataPtr->camera->PreRender(); this->textureId = this->dataPtr->camera->RenderTextureGLId(); + // Make service call to populate scene + if (!this->sceneService.empty()) + { + this->dataPtr->sceneManager.Load(this->sceneService, this->poseTopic, + this->deletionTopic, this->sceneTopic, + scene); + this->dataPtr->sceneManager.Request(); + } + // Ray Query this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); @@ -621,16 +1350,14 @@ void RenderWindowItem::SetSceneTopic(const std::string &_topic) this->dataPtr->renderThread->ignRenderer.sceneTopic = _topic; } -///////////////////////////////////////////////// -void RenderWindowItem::SetSkyEnabled(const bool &_sky) -{ - this->dataPtr->renderThread->ignRenderer.skyEnable = _sky; -} - ///////////////////////////////////////////////// Scene3D::Scene3D() : Plugin(), dataPtr(new Scene3DPrivate) { + ignwarn << "This plugin is deprecated on ign-gui v6 and will be removed on " + << "ign-gui v7. Use MinimalScene + TransportSceneManager instead." + << std::endl; + qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); } @@ -730,14 +1457,6 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) std::string topic = elem->GetText(); renderWindow->SetSceneTopic(topic); } - - elem = _pluginElem->FirstChildElement("sky"); - if (nullptr != elem && nullptr != elem->GetText()) - { - renderWindow->SetSkyEnabled(true); - if (!elem->NoChildren()) - ignwarn << "Child elements of are not supported yet" << std::endl; - } } } @@ -791,6 +1510,24 @@ void RenderWindowItem::wheelEvent(QWheelEvent *_e) this->dataPtr->mouseEvent, math::Vector2d(scroll, scroll)); } +/////////////////////////////////////////////////// +// void Scene3D::resizeEvent(QResizeEvent *_e) +// { +// if (this->dataPtr->renderWindow) +// { +// this->dataPtr->renderWindow->OnResize(_e->size().width(), +// _e->size().height()); +// } +// +// if (this->dataPtr->camera) +// { +// this->dataPtr->camera->SetAspectRatio( +// static_cast(this->width()) / this->height()); +// this->dataPtr->camera->SetHFOV(M_PI * 0.5); +// } +// } +// + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gui::plugins::Scene3D, ignition::gui::Plugin) diff --git a/src/plugins/scene3d/Scene3D.hh b/src/plugins/scene3d/Scene3D.hh index 7d199e0c4..1c51054be 100644 --- a/src/plugins/scene3d/Scene3D.hh +++ b/src/plugins/scene3d/Scene3D.hh @@ -160,9 +160,6 @@ namespace plugins /// added public: std::string sceneTopic; - /// \brief True if sky is enabled; - public: bool skyEnable = false; - /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -257,10 +254,6 @@ namespace plugins /// \param[in] _topic Scene topic public: void SetSceneTopic(const std::string &_topic); - /// \brief Set if sky is enabled - /// \param[in] _sky True to enable the sky, false otherwise. - public: void SetSkyEnabled(const bool &_sky); - /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); From c50f5175ce5fd317e45f8d78176698b8ff9b6cbd Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 18 Jun 2021 17:16:58 -0700 Subject: [PATCH 11/23] Add tests Signed-off-by: Louise Poubel --- .../TransportSceneManager.cc | 15 ++ test/CMakeLists.txt | 1 + test/helpers/CMakeLists.txt | 15 ++ test/helpers/TestHelper.cc | 38 +++ test/helpers/TestHelper.hh | 57 +++++ test/integration/CMakeLists.txt | 12 +- test/integration/minimal_scene.cc | 128 ++++++++++ test/integration/transport_scene_manager.cc | 233 ++++++++++++++++++ 8 files changed, 498 insertions(+), 1 deletion(-) create mode 100644 test/helpers/CMakeLists.txt create mode 100644 test/helpers/TestHelper.cc create mode 100644 test/helpers/TestHelper.hh create mode 100644 test/integration/minimal_scene.cc create mode 100644 test/integration/transport_scene_manager.cc diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.cc b/src/plugins/transport_scene_manager/TransportSceneManager.cc index 26acd318a..3e52fb746 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.cc +++ b/src/plugins/transport_scene_manager/TransportSceneManager.cc @@ -236,6 +236,11 @@ void TransportSceneManagerPrivate::InitializeTransport() ignerr << "Error subscribing to pose topic: " << this->poseTopic << std::endl; } + else + { + ignmsg << "Listening to pose messages on [" << this->poseTopic << "]" + << std::endl; + } if (!this->node.Subscribe(this->deletionTopic, &TransportSceneManagerPrivate::OnDeletionMsg, this)) @@ -243,6 +248,11 @@ void TransportSceneManagerPrivate::InitializeTransport() ignerr << "Error subscribing to deletion topic: " << this->deletionTopic << std::endl; } + else + { + ignmsg << "Listening to deletion messages on [" << this->deletionTopic << "]" + << std::endl; + } if (!this->node.Subscribe(this->sceneTopic, &TransportSceneManagerPrivate::OnSceneMsg, this)) @@ -250,6 +260,11 @@ void TransportSceneManagerPrivate::InitializeTransport() ignerr << "Error subscribing to scene topic: " << this->sceneTopic << std::endl; } + else + { + ignmsg << "Listening to scene messages on [" << this->sceneTopic << "]" + << std::endl; + } ignmsg << "Transport initialized." << std::endl; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6dd8ad952..d0d40bd18 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -18,6 +18,7 @@ execute_process(COMMAND cmake -E remove_directory ${CMAKE_BINARY_DIR}/test_resul execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results) include_directories(${GTEST_INCLUDE_DIRS}) +add_subdirectory(helpers) add_subdirectory(integration) add_subdirectory(performance) add_subdirectory(plugins) diff --git a/test/helpers/CMakeLists.txt b/test/helpers/CMakeLists.txt new file mode 100644 index 000000000..46af75c2a --- /dev/null +++ b/test/helpers/CMakeLists.txt @@ -0,0 +1,15 @@ +set (qt_headers + TestHelper.hh +) + +QT5_WRAP_CPP(headers_MOC ${qt_headers}) + +add_library(${PROJECT_NAME}_test_helpers SHARED + TestHelper.cc + ${headers_MOC} +) +target_compile_definitions(${PROJECT_NAME}_test_helpers PRIVATE TestHelper_EXPORTS) +target_link_libraries(${PROJECT_NAME}_test_helpers + PUBLIC + ${PROJECT_LIBRARY_TARGET_NAME} +) diff --git a/test/helpers/TestHelper.cc b/test/helpers/TestHelper.cc new file mode 100644 index 000000000..e7d70984d --- /dev/null +++ b/test/helpers/TestHelper.cc @@ -0,0 +1,38 @@ +/* + * 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 "TestHelper.hh" + +namespace ignition +{ +namespace gui +{ +TestHelper::TestHelper() +{ + App()->findChild()->installEventFilter(this); +} + +bool TestHelper::eventFilter(QObject *_obj, QEvent *_event) +{ + if (this->forwardEvent) + this->forwardEvent(_event); + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} +} +} diff --git a/test/helpers/TestHelper.hh b/test/helpers/TestHelper.hh new file mode 100644 index 000000000..8c2fee0f9 --- /dev/null +++ b/test/helpers/TestHelper.hh @@ -0,0 +1,57 @@ +/* + * 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_GUI_TESTHELPER_HH_ +#define IGNITION_GUI_TESTHELPER_HH_ + +#include +#include +#include + +#ifndef _WIN32 +# define TestHelper_EXPORTS_API +#else +# if (defined(TestHelper_EXPORTS)) +# define TestHelper_EXPORTS_API __declspec(dllexport) +# else +# define TestHelper_EXPORTS_API __declspec(dllimport) +# endif +#endif + +namespace ignition +{ +namespace gui +{ +/// \brief +class TestHelper_EXPORTS_API TestHelper : public QObject +{ + Q_OBJECT + + /// \brief Constructor + public: TestHelper(); + + /// \brief Destructor + public: ~TestHelper() = default; + + /// \brief Documentation inherited + public: bool eventFilter(QObject *_obj, QEvent *_event) override; + + public: std::function forwardEvent; +}; +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 397fabacb..aeb5c8f1f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,3 +1,13 @@ ign_get_sources(tests) -ign_build_tests(TYPE INTEGRATION SOURCES ${tests}) +find_package(Qt5Test REQUIRED) + +ign_build_tests( + TYPE INTEGRATION + SOURCES ${tests} + LIB_DEPS + ${PROJECT_NAME}_test_helpers + ignition-plugin${IGN_PLUGIN_VER}::loader + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + Qt5::Test +) diff --git a/test/integration/minimal_scene.cc b/test/integration/minimal_scene.cc new file mode 100644 index 000000000..897bf28f5 --- /dev/null +++ b/test/integration/minimal_scene.cc @@ -0,0 +1,128 @@ +/* + * 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 "test_config.h" // NOLINT(build/include) +#include "../helpers/TestHelper.hh" +#include "ignition/gui/Application.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/Plugin.hh" +#include "ignition/gui/MainWindow.hh" + +int g_argc = 1; +char **g_argv = new char *[g_argc]; + +using namespace ignition; +using namespace gui; + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) +{ + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + EXPECT_TRUE(app.LoadPlugin("MinimalScene")); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + EXPECT_EQ(plugins.size(), 1); + + auto plugin = plugins[0]; + EXPECT_EQ(plugin->Title(), "3D Scene"); + + // Cleanup + plugins.clear(); +} + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) +{ + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Load plugin + const char *pluginStr = + "" + "ogre" + "banana" + "1.0 0 0" + "0 1 0" + "1 2 3 0 0 1.57" + ""; + + tinyxml2::XMLDocument pluginDoc; + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("MinimalScene", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Check scene + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + int sleep = 0; + int maxSleep = 30; + while (0 == engine->SceneCount() && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_EQ(1u, engine->SceneCount()); + auto scene = engine->SceneByName("banana"); + ASSERT_NE(nullptr, scene); + + EXPECT_EQ(math::Color(0, 1, 0), scene->BackgroundColor()); + EXPECT_EQ(math::Color(1, 0, 0), scene->AmbientLight()); + + auto root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + EXPECT_EQ(1u, root->ChildCount()); + + // Check camera + auto camera = std::dynamic_pointer_cast( + root->ChildByIndex(0)); + ASSERT_NE(nullptr, camera); + + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1.57), camera->WorldPose()); +} + diff --git a/test/integration/transport_scene_manager.cc b/test/integration/transport_scene_manager.cc new file mode 100644 index 000000000..47bb25104 --- /dev/null +++ b/test/integration/transport_scene_manager.cc @@ -0,0 +1,233 @@ +/* + * 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 +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "../helpers/TestHelper.hh" +#include "ignition/gui/Application.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/Plugin.hh" +#include "ignition/gui/MainWindow.hh" + +int g_argc = 1; +char **g_argv = new char *[g_argc]; + +using namespace ignition; +using namespace gui; + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) +{ + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + EXPECT_TRUE(app.LoadPlugin("MinimalScene")); + EXPECT_TRUE(app.LoadPlugin("TransportSceneManager")); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + EXPECT_EQ(plugins.size(), 2); + + auto plugin = plugins[0]; + EXPECT_EQ(plugin->Title(), "3D Scene"); + + plugin = plugins[1]; + EXPECT_EQ(plugin->Title(), "Transport Scene Manager"); + + // Cleanup + plugins.clear(); +} + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) +{ + bool sceneRequested{false}; + std::function sceneService = + [&](msgs::Scene &_rep) -> bool + { + auto modelMsg = _rep.add_model(); + modelMsg->set_id(1); + modelMsg->set_is_static(true); + modelMsg->set_name("box_model"); + + auto linkMsg = modelMsg->add_link(); + linkMsg->set_id(2); + linkMsg->set_name("box_link"); + + auto visMsg = linkMsg->add_visual(); + visMsg->set_id(3); + visMsg->set_name("box_vis"); + + auto geomMsg = visMsg->mutable_geometry(); + auto boxMsg = geomMsg->mutable_box(); + auto boxSize = boxMsg->mutable_size(); + boxSize->set_x(1.0); + boxSize->set_y(2.0); + boxSize->set_z(3.0); + + sceneRequested = true; + return true; + }; + + // Scene service + transport::Node node; + node.Advertise("/test/scene", sceneService); + + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Load plugins + const char *pluginStr = + "" + "ogre" + "banana" + "1.0 0 0" + "0 1 0" + "1 2 3 0 0 1.57" + ""; + + tinyxml2::XMLDocument pluginDoc; + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("MinimalScene", + pluginDoc.FirstChildElement("plugin"))); + + pluginStr = + "" + "/test/scene" + "/test/pose" + "/test/delete" + "/test/scene" + ""; + + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("TransportSceneManager", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Get scene + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + int sleep = 0; + int maxSleep = 30; + while (!sceneRequested && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + auto scene = engine->SceneByName("banana"); + ASSERT_NE(nullptr, scene); + + auto root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // Check scene is populated + EXPECT_EQ(2u, root->ChildCount()); + + // First child is user camera + auto camera = std::dynamic_pointer_cast( + root->ChildByIndex(0)); + EXPECT_NE(nullptr, camera); + + // Check box + auto modelVis = std::dynamic_pointer_cast( + root->ChildByIndex(1)); + ASSERT_NE(nullptr, modelVis); + EXPECT_EQ(math::Pose3d::Zero, modelVis->LocalPose()); + EXPECT_EQ(1u, modelVis->ChildCount()); + EXPECT_EQ(0u, modelVis->GeometryCount()); + + auto linkVis = std::dynamic_pointer_cast( + modelVis->ChildByIndex(0)); + ASSERT_NE(nullptr, linkVis); + EXPECT_EQ(math::Pose3d::Zero, linkVis->LocalPose()); + EXPECT_EQ(1u, linkVis->ChildCount()); + EXPECT_EQ(0u, linkVis->GeometryCount()); + + auto visualVis = std::dynamic_pointer_cast( + linkVis->ChildByIndex(0)); + ASSERT_NE(nullptr, visualVis); + EXPECT_EQ(math::Pose3d::Zero, visualVis->LocalPose()); + EXPECT_EQ(0u, visualVis->ChildCount()); + EXPECT_EQ(1u, visualVis->GeometryCount()); + + // Change model pose + auto posePub = node.Advertise("/test/pose"); + + ignition::msgs::Pose_V poseVMsg; + auto poseMsg = poseVMsg.add_pose(); + poseMsg->set_id(1); + poseMsg->set_name("box_model"); + auto positionMsg = poseMsg->mutable_position(); + positionMsg->set_x(5); + posePub.Publish(poseVMsg); + + sleep = 0; + while (modelVis->LocalPose() == math::Pose3d::Zero && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + EXPECT_EQ(math::Pose3d(5, 0, 0, 0, 0, 0), modelVis->LocalPose()); + + // Delete model + auto deletePub = node.Advertise("/test/delete"); + + ignition::msgs::UInt32_V entityVMsg; + entityVMsg.add_data(1); + deletePub.Publish(entityVMsg); + + sleep = 0; + while (root->ChildCount() > 1 && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + EXPECT_EQ(1u, root->ChildCount()); +} + From 0c4e312a5564ab0f6caba3915b6565f9b6ea64b2 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 18 Jun 2021 19:29:49 -0700 Subject: [PATCH 12/23] Add tutorial Signed-off-by: Louise Poubel --- CMakeLists.txt | 5 +++- tutorials.md.in | 20 ++++++++------ tutorials/scene.md | 65 ++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 81 insertions(+), 9 deletions(-) create mode 100644 tutorials/scene.md diff --git a/CMakeLists.txt b/CMakeLists.txt index d0da1ab4e..4981e681b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -116,8 +116,11 @@ ign_create_docs( "\"${CMAKE_SOURCE_DIR}/doc/qt.tag.xml=http://doc.qt.io/qt-5/\" \ \"${IGNITION-MATH_DOXYGEN_TAGFILE} = ${IGNITION-MATH_API_URL}\" \ \"${IGNITION-MSGS_DOXYGEN_TAGFILE} = ${IGNITION-MSGS_API_URL}\" \ + \"${IGNITION-RENDERING_DOXYGEN_TAGFILE} = ${IGNITION-RENDERING_API_URL}\" \ \"${IGNITION-TRANSPORT_DOXYGEN_TAGFILE} = ${IGNITION-TRANSPORT_API_URL}\" \ \"${IGNITION-COMMON_DOXYGEN_TAGFILE} = ${IGNITION-COMMON_API_URL}\"" ) -file(COPY ${CMAKE_SOURCE_DIR}/tutorials/images/ DESTINATION ${CMAKE_BINARY_DIR}/doxygen/html/images/) +if(TARGET doc) + file(COPY ${CMAKE_SOURCE_DIR}/tutorials/images/ DESTINATION ${CMAKE_BINARY_DIR}/doxygen/html/images/) +endif() diff --git a/tutorials.md.in b/tutorials.md.in index 5c5df1d22..08245746d 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -5,16 +5,20 @@ will guide you through the process of understanding the capabilities of the Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. -**The tutorials** +**General** 1. \subpage install "Installation" -2. \subpage commandline "Command line usage" -3. \subpage plugins "Plugins" -4. \subpage config "Configuration files" -5. \subpage layout "Layout" -6. \subpage style "Style" -7. \subpage examples "Examples" -8. \subpage screenshot "Screenshot" +2. \subpage commandline "Command line usage": Learn to use `ign gui` +3. \subpage plugins "Plugins": Introduction to writing and loading plugins +4. \subpage config "Configuration files": Load and find config files +5. \subpage layout "Layout": Arrange plugins on the application +6. \subpage style "Style": Customize the application's look +7. \subpage examples "Examples": Navigate the examples folder + +**Plugins** + +1. \subpage scene "3D Scene": How to use the rendering scene +2. \subpage screenshot "Screenshot": Save screenshots of the 3D scene ## License diff --git a/tutorials/scene.md b/tutorials/scene.md new file mode 100644 index 000000000..f08279266 --- /dev/null +++ b/tutorials/scene.md @@ -0,0 +1,65 @@ +\page scene 3D Scene + +## Overview + +Applications built on top of Ignition GUI can leverage Ignition Rendering +to display interactive 3D scenes. This tutorial will help application developers +leverage Ignition GUI's plugins to quickly develop plugins that interact with +the 3D scene. + +> This tutorial applies for Ignition GUI version 6 (Fortress) and higher. + +## Minimal scene + +Ignition GUI ships with the `MinimalScene` plugin, which instantiates a 3D +scene and provides orbit controls, but doesn't do much else. Actions such as +adding, modifying and removing rendering elements from the scene must be +performed by other plugins that work alongside the minimal scene. + +Each application will have a different way of updating the 3D scene. For example, +[Ignition Gazebo](https://ignitionrobotics.org/libs/gazebo) updates the scene +based on its entities and components, and +[Ignition RViz](https://github.com/ignitionrobotics/ign-rviz/) +updates the scene based on ROS 2 messages. Each of these applications provides +custom plugins that update the 3D scene through events and the render thread. + +Ignition GUI ships with a plugin that updates the scene based on Ignition +Transport messages, the `ignition::gui::plugins::TransportSceneManager`. +Applications can use that directly, or use it as inspiration for developing +their own scene managers. + +## Transport scene manager example + +Let's start by loading `MinimalScene` together with `TransportSceneManager`. +We'll use a simple program to send messages to `TransportSceneManager`, which +will process those and update the 3D scene, and then the `MinimalScene` will +paint the scene to the window. + +Follow the instructions in the +[scene_provider example](https://github.com/ignitionrobotics/ign-gui/tree/main/examples/standalone/scene_provider) +and see visuals being added and moved on the 3D scene. + +### Getting the scene + +After the `MinimalScene` instantiates an `ignition::rendering::Scene` pointer, +any plugin can get it using the rendering engine's singleton. The +`ignition::rendering::sceneFromFirstRenderEngine()` +function is a convenient way of getting the scene for applications that +have a single scene loaded. + +The `TransportSceneManager` plugin, for example, attempts to get the scene at +every iteration until it succeeds. This way, it can find a 3D scene even if +`MinimalScene` is loaded afterwards. + +### Render thread + +Rendering operations aren't thread-safe. To make sure there are no race +conditions, all rendering operations should happen in the same thread, the +"render thread". In order to access that thread from a custom plugin, it's +necessary to listen to the `ignition::gui::events::Render` event, which is +emitted by the `MinimalScene`. + +See how the `TransportSceneManager` installs an event filter with +`installEventFilter`, registers an `eventFilter` callback and performs all +rendering operations within the `OnRender` function. + From 6d3690db7cb072664a7382752654763c7cee5a34 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 18 Jun 2021 19:34:27 -0700 Subject: [PATCH 13/23] codecheck Signed-off-by: Louise Poubel --- src/plugins/transport_scene_manager/TransportSceneManager.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.cc b/src/plugins/transport_scene_manager/TransportSceneManager.cc index 3e52fb746..b3ac520f7 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.cc +++ b/src/plugins/transport_scene_manager/TransportSceneManager.cc @@ -250,8 +250,8 @@ void TransportSceneManagerPrivate::InitializeTransport() } else { - ignmsg << "Listening to deletion messages on [" << this->deletionTopic << "]" - << std::endl; + ignmsg << "Listening to deletion messages on [" << this->deletionTopic + << "]" << std::endl; } if (!this->node.Subscribe(this->sceneTopic, From 189608078fc856cee1e305b84f68da9c397a7cda Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 24 Jun 2021 15:58:05 -0700 Subject: [PATCH 14/23] Update plugins with forward merge, improve pose example Signed-off-by: Louise Poubel --- .../scene_provider/scene_provider.cc | 18 +- src/plugins/minimal_scene/MinimalScene.cc | 185 ++++++++++++++++++ src/plugins/minimal_scene/MinimalScene.hh | 49 +++++ src/plugins/minimal_scene/MinimalScene.qml | 17 +- .../TransportSceneManager.cc | 20 ++ 5 files changed, 284 insertions(+), 5 deletions(-) diff --git a/examples/standalone/scene_provider/scene_provider.cc b/examples/standalone/scene_provider/scene_provider.cc index 2978cb003..5f32eab8e 100644 --- a/examples/standalone/scene_provider/scene_provider.cc +++ b/examples/standalone/scene_provider/scene_provider.cc @@ -84,13 +84,23 @@ int main(int argc, char **argv) poseMsg->set_name("box_model"); auto positionMsg = poseMsg->mutable_position(); + const double change{0.1}; + + double x{0.0}; + double y{0.0}; + double z{0.0}; + while (true) { - std::this_thread::sleep_for(1s); + std::this_thread::sleep_for(100ms); + + x += ignition::math::Rand::DblUniform(-change, change); + y += ignition::math::Rand::DblUniform(-change, change); + z += ignition::math::Rand::DblUniform(-change, change); - positionMsg->set_x(ignition::math::Rand::DblUniform(-3.0, 3.0)); - positionMsg->set_y(ignition::math::Rand::DblUniform(-3.0, 3.0)); - positionMsg->set_z(ignition::math::Rand::DblUniform(-3.0, 3.0)); + positionMsg->set_x(x); + positionMsg->set_y(y); + positionMsg->set_z(z); posePub.Publish(poseVMsg); } diff --git a/src/plugins/minimal_scene/MinimalScene.cc b/src/plugins/minimal_scene/MinimalScene.cc index d003d4f54..d404b2fd5 100644 --- a/src/plugins/minimal_scene/MinimalScene.cc +++ b/src/plugins/minimal_scene/MinimalScene.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -65,9 +66,15 @@ namespace plugins /// \brief Flag to indicate if mouse event is dirty public: bool mouseDirty = false; + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty = false; + /// \brief Mouse event public: common::MouseEvent mouseEvent; + /// \brief Key event + public: common::KeyEvent keyEvent; + /// \brief Mouse move distance since last event. public: math::Vector2d drag; @@ -80,6 +87,9 @@ namespace plugins /// \brief Camera orbit controller public: rendering::OrbitViewController viewControl; + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos{math::Vector2i::Zero}; + /// \brief Ray query for mouse clicks public: rendering::RayQueryPtr rayQuery; @@ -160,6 +170,15 @@ void IgnRenderer::Render() void IgnRenderer::HandleMouseEvent() { std::lock_guard lock(this->dataPtr->mutex); + this->BroadcastHoverPos(); + this->BroadcastLeftClick(); + this->BroadcastRightClick(); + this->HandleMouseViewControl(); +} + +///////////////////////////////////////////////// +void IgnRenderer::HandleMouseViewControl() +{ if (!this->dataPtr->mouseDirty) return; @@ -214,6 +233,106 @@ void IgnRenderer::HandleMouseEvent() this->dataPtr->mouseDirty = false; } +//////////////////////////////////////////////// +void IgnRenderer::HandleKeyPress(QKeyEvent *_e) +{ + if (_e->isAutoRepeat()) + return; + + std::lock_guard lock(this->dataPtr->mutex); + + this->dataPtr->keyEvent.SetKey(_e->key()); + this->dataPtr->keyEvent.SetText(_e->text().toStdString()); + + this->dataPtr->keyEvent.SetControl( + (_e->modifiers() & Qt::ControlModifier)); + this->dataPtr->keyEvent.SetShift( + (_e->modifiers() & Qt::ShiftModifier)); + this->dataPtr->keyEvent.SetAlt( + (_e->modifiers() & Qt::AltModifier)); + + this->dataPtr->mouseEvent.SetControl(this->dataPtr->keyEvent.Control()); + this->dataPtr->mouseEvent.SetShift(this->dataPtr->keyEvent.Shift()); + this->dataPtr->mouseEvent.SetAlt(this->dataPtr->keyEvent.Alt()); + this->dataPtr->keyEvent.SetType(common::KeyEvent::PRESS); +} + +//////////////////////////////////////////////// +void IgnRenderer::HandleKeyRelease(QKeyEvent *_e) +{ + if (_e->isAutoRepeat()) + return; + + std::lock_guard lock(this->dataPtr->mutex); + + this->dataPtr->keyEvent.SetKey(0); + + this->dataPtr->keyEvent.SetControl( + (_e->modifiers() & Qt::ControlModifier) + && (_e->key() != Qt::Key_Control)); + this->dataPtr->keyEvent.SetShift( + (_e->modifiers() & Qt::ShiftModifier) + && (_e->key() != Qt::Key_Shift)); + this->dataPtr->keyEvent.SetAlt( + (_e->modifiers() & Qt::AltModifier) + && (_e->key() != Qt::Key_Alt)); + + this->dataPtr->mouseEvent.SetControl(this->dataPtr->keyEvent.Control()); + this->dataPtr->mouseEvent.SetShift(this->dataPtr->keyEvent.Shift()); + this->dataPtr->mouseEvent.SetAlt(this->dataPtr->keyEvent.Alt()); + this->dataPtr->keyEvent.SetType(common::KeyEvent::RELEASE); +} + +///////////////////////////////////////////////// +void IgnRenderer::BroadcastHoverPos() +{ + if (!this->dataPtr->hoverDirty) + return; + + auto pos = this->ScreenToScene(this->dataPtr->mouseHoverPos); + + events::HoverToScene hoverToSceneEvent(pos); + App()->sendEvent(App()->findChild(), &hoverToSceneEvent); +} + +///////////////////////////////////////////////// +void IgnRenderer::BroadcastLeftClick() +{ + if (!this->dataPtr->mouseDirty) + return; + + if (this->dataPtr->mouseEvent.Dragging()) + return; + + if (this->dataPtr->mouseEvent.Button() != common::MouseEvent::LEFT || + this->dataPtr->mouseEvent.Type() != common::MouseEvent::RELEASE) + return; + + auto pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); + + events::LeftClickToScene leftClickToSceneEvent(pos); + App()->sendEvent(App()->findChild(), &leftClickToSceneEvent); +} + +///////////////////////////////////////////////// +void IgnRenderer::BroadcastRightClick() +{ + if (!this->dataPtr->mouseDirty) + return; + + if (this->dataPtr->mouseEvent.Dragging()) + return; + + if (this->dataPtr->mouseEvent.Button() != common::MouseEvent::RIGHT || + this->dataPtr->mouseEvent.Type() != common::MouseEvent::RELEASE) + return; + + auto pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); + + events::RightClickToScene rightClickToSceneEvent(pos); + App()->sendEvent(App()->findChild(), &rightClickToSceneEvent); +} + ///////////////////////////////////////////////// void IgnRenderer::Initialize() { @@ -287,6 +406,14 @@ void IgnRenderer::Destroy() } } +///////////////////////////////////////////////// +void IgnRenderer::NewHoverEvent(const math::Vector2i &_hoverPos) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->mouseHoverPos = _hoverPos; + this->dataPtr->hoverDirty = true; +} + ///////////////////////////////////////////////// void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag) @@ -741,6 +868,12 @@ void MinimalScene::LoadConfig(const tinyxml2::XMLElement *_pluginElem) } } +///////////////////////////////////////////////// +void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) +{ + this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); +} + ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { @@ -791,6 +924,58 @@ void RenderWindowItem::wheelEvent(QWheelEvent *_e) this->dataPtr->mouseEvent, math::Vector2d(scroll, scroll)); } +//////////////////////////////////////////////// +void RenderWindowItem::HandleKeyPress(QKeyEvent *_e) +{ + this->dataPtr->renderThread->ignRenderer.HandleKeyPress(_e); +} + +//////////////////////////////////////////////// +void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e) +{ + this->dataPtr->renderThread->ignRenderer.HandleKeyRelease(_e); +} + +///////////////////////////////////////////////// +bool MinimalScene::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->HandleKeyPress(keyEvent); + } + } + else if (_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->HandleKeyRelease(keyEvent); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void MinimalScene::OnHovered(int _mouseX, int _mouseY) +{ + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->OnHovered({_mouseX, _mouseY}); +} + +///////////////////////////////////////////////// +void MinimalScene::OnFocusWindow() +{ + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->forceActiveFocus(); +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gui::plugins::MinimalScene, ignition::gui::Plugin) diff --git a/src/plugins/minimal_scene/MinimalScene.hh b/src/plugins/minimal_scene/MinimalScene.hh index 2319e0869..ead6a2def 100644 --- a/src/plugins/minimal_scene/MinimalScene.hh +++ b/src/plugins/minimal_scene/MinimalScene.hh @@ -68,6 +68,18 @@ namespace plugins /// \brief Destructor public: virtual ~MinimalScene(); + /// \brief Callback when the mouse hovers to a new position. + /// \param[in] _mouseX x coordinate of the hovered mouse position. + /// \param[in] _mouseY y coordinate of the hovered mouse position. + public slots: void OnHovered(int _mouseX, int _mouseY); + + /// \brief Callback when the mouse enters the render window to + /// focus the window for mouse/key events + public slots: void OnFocusWindow(); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + // Documentation inherited public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; @@ -106,9 +118,33 @@ namespace plugins public: void NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag = math::Vector2d::Zero); + /// \brief New hover event triggered. + /// \param[in] _hoverPos Mouse hover screen position + public: void NewHoverEvent(const math::Vector2i &_hoverPos); + + /// \brief Handle key press event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyPress(QKeyEvent *_e); + + /// \brief Handle key release event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyRelease(QKeyEvent *_e); + /// \brief Handle mouse event for view control private: void HandleMouseEvent(); + /// \brief Handle mouse event for view control + private: void HandleMouseViewControl(); + + /// \brief Broadcasts the currently hovered 3d scene location. + private: void BroadcastHoverPos(); + + /// \brief Broadcasts a left click within the scene + private: void BroadcastLeftClick(); + + /// \brief Broadcasts a right click within the scene + private: void BroadcastRightClick(); + /// \brief Retrieve the first point on a surface in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. /// \param[in] _screenPos 2D coordinates on the screen, in pixels. @@ -257,6 +293,11 @@ namespace plugins /// \param[in] _topic Scene topic public: void SetSceneTopic(const std::string &_topic); + /// \brief Called when the mouse hovers to a new position. + /// \param[in] _hoverPos 2D coordinates of the hovered mouse position on + /// the render window. + public: void OnHovered(const ignition::math::Vector2i &_hoverPos); + /// \brief Set if sky is enabled /// \param[in] _sky True to enable the sky, false otherwise. public: void SetSkyEnabled(const bool &_sky); @@ -264,6 +305,14 @@ namespace plugins /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); + /// \brief Handle key press event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyPress(QKeyEvent *_e); + + /// \brief Handle key release event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyRelease(QKeyEvent *_e); + // Documentation inherited protected: virtual void mousePressEvent(QMouseEvent *_e) override; diff --git a/src/plugins/minimal_scene/MinimalScene.qml b/src/plugins/minimal_scene/MinimalScene.qml index 304ccf186..bab4a5d16 100644 --- a/src/plugins/minimal_scene/MinimalScene.qml +++ b/src/plugins/minimal_scene/MinimalScene.qml @@ -14,7 +14,7 @@ * limitations under the License. * */ -import QtQuick 2.0 +import QtQuick 2.9 import QtQuick.Controls 2.0 import RenderWindow 1.0 import QtGraphicalEffects 1.0 @@ -28,6 +28,21 @@ Rectangle { */ property bool gammaCorrect: false + /** + * Get mouse position on 3D widget + */ + MouseArea { + id: mouseArea + anchors.fill: parent + hoverEnabled: true + acceptedButtons: Qt.NoButton + onEntered: { + MinimalScene.OnFocusWindow() + } + onPositionChanged: { + MinimalScene.OnHovered(mouseArea.mouseX, mouseArea.mouseY); + } + } RenderWindow { id: renderWindow diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.cc b/src/plugins/transport_scene_manager/TransportSceneManager.cc index b3ac520f7..1f81e6e0a 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.cc +++ b/src/plugins/transport_scene_manager/TransportSceneManager.cc @@ -28,6 +28,8 @@ #include #include +#include + #include #include @@ -640,6 +642,24 @@ rendering::GeometryPtr TransportSceneManagerPrivate::LoadGeometry( scale.Y() = scale.X(); scale.Z() = _msg.cylinder().length(); } + else if (_msg.has_capsule()) + { + auto capsule = this->scene->CreateCapsule(); + capsule->SetRadius(_msg.capsule().radius()); + capsule->SetLength(_msg.capsule().length()); + geom = capsule; + + scale.X() = _msg.capsule().radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _msg.capsule().length() + scale.X(); + } + else if (_msg.has_ellipsoid()) + { + geom = this->scene->CreateSphere(); + scale.X() = _msg.ellipsoid().radii().x() * 2; + scale.Y() = _msg.ellipsoid().radii().y() * 2; + scale.Z() = _msg.ellipsoid().radii().z() * 2; + } else if (_msg.has_plane()) { geom = this->scene->CreatePlane(); From 8a3f62f2d224011f8b0fc4b793714ce39e5603fa Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 24 Jun 2021 16:24:56 -0700 Subject: [PATCH 15/23] PR feedback Signed-off-by: Louise Poubel --- src/plugins/minimal_scene/CMakeLists.txt | 2 - src/plugins/minimal_scene/MinimalScene.cc | 12 +- src/plugins/minimal_scene/MinimalScene.hh | 7 +- .../minimal_scene/MinimalScene_TEST.cc | 122 ------------------ .../TransportSceneManager.cc | 16 +-- .../TransportSceneManager.hh | 11 +- test/integration/minimal_scene.cc | 14 ++ test/integration/transport_scene_manager.cc | 20 +++ 8 files changed, 54 insertions(+), 150 deletions(-) delete mode 100644 src/plugins/minimal_scene/MinimalScene_TEST.cc diff --git a/src/plugins/minimal_scene/CMakeLists.txt b/src/plugins/minimal_scene/CMakeLists.txt index 9843181db..7f98b9908 100644 --- a/src/plugins/minimal_scene/CMakeLists.txt +++ b/src/plugins/minimal_scene/CMakeLists.txt @@ -3,8 +3,6 @@ ign_gui_add_plugin(MinimalScene MinimalScene.cc QT_HEADERS MinimalScene.hh - TEST_SOURCES - # MinimalScene_TEST.cc PUBLIC_LINK_LIBS ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ) diff --git a/src/plugins/minimal_scene/MinimalScene.cc b/src/plugins/minimal_scene/MinimalScene.cc index d404b2fd5..a614947f7 100644 --- a/src/plugins/minimal_scene/MinimalScene.cc +++ b/src/plugins/minimal_scene/MinimalScene.cc @@ -18,7 +18,6 @@ #include "MinimalScene.hh" #include -#include #include #include #include @@ -27,17 +26,16 @@ #include #include #include -#include -#include - #include #include +#include // TODO(louise) Remove these pragmas once ign-rendering // is disabling the warnings #ifdef _MSC_VER #pragma warning(push, 0) #endif + #include #include #include @@ -91,7 +89,7 @@ namespace plugins public: math::Vector2i mouseHoverPos{math::Vector2i::Zero}; /// \brief Ray query for mouse clicks - public: rendering::RayQueryPtr rayQuery; + public: rendering::RayQueryPtr rayQuery{nullptr}; /// \brief View control focus target public: math::Vector3d target; @@ -375,7 +373,7 @@ void IgnRenderer::Initialize() this->dataPtr->camera->SetAntiAliasing(8); this->dataPtr->camera->SetHFOV(M_PI * 0.5); // setting the size and calling PreRender should cause the render texture to - // be rebuilt + // be rebuilt this->dataPtr->camera->PreRender(); this->textureId = this->dataPtr->camera->RenderTextureGLId(); @@ -494,7 +492,6 @@ void RenderThread::ShutDown() this->moveToThread(QGuiApplication::instance()->thread()); } - ///////////////////////////////////////////////// void RenderThread::SizeChanged() { @@ -761,7 +758,6 @@ MinimalScene::MinimalScene() qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); } - ///////////////////////////////////////////////// MinimalScene::~MinimalScene() { diff --git a/src/plugins/minimal_scene/MinimalScene.hh b/src/plugins/minimal_scene/MinimalScene.hh index ead6a2def..95935baad 100644 --- a/src/plugins/minimal_scene/MinimalScene.hh +++ b/src/plugins/minimal_scene/MinimalScene.hh @@ -20,16 +20,12 @@ #include #include -#include +#include #include #include #include -#include - -#include -#include "ignition/gui/qt.h" #include "ignition/gui/Plugin.hh" namespace ignition @@ -58,6 +54,7 @@ namespace plugins /// (0.3, 0.3, 0.3, 1.0) /// * \ : Optional starting pose for the camera, defaults to /// (0, 0, 5, 0, 0, 0) + /// * \ : If present, sky is enabled. class MinimalScene : public Plugin { Q_OBJECT diff --git a/src/plugins/minimal_scene/MinimalScene_TEST.cc b/src/plugins/minimal_scene/MinimalScene_TEST.cc deleted file mode 100644 index 0c798904a..000000000 --- a/src/plugins/minimal_scene/MinimalScene_TEST.cc +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright (C) 2017 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 "ignition/gui/Iface.hh" -#include "ignition/gui/MainWindow.hh" -#include "ignition/gui/Plugin.hh" - -using namespace ignition; -using namespace gui; - -///////////////////////////////////////////////// -TEST(MinimalSceneTest, Load) -{ - setVerbosity(4); - EXPECT_TRUE(initApp()); - - EXPECT_TRUE(loadPlugin("MinimalScene")); - - EXPECT_TRUE(stop()); -} - -///////////////////////////////////////////////// -TEST(MinimalSceneTest, Resize) -{ - setVerbosity(4); - EXPECT_TRUE(initApp()); - - // Load plugin - EXPECT_TRUE(loadPlugin("MinimalScene")); - - // Create main window - EXPECT_TRUE(createMainWindow()); - - // Close window after some time - auto win = mainWindow(); - ASSERT_NE(nullptr, win); - - QTimer::singleShot(300, [&win]() - { - // Check there are no segfaults when resizing - for (auto i : {100, 300, 200, 500, 400}) - { - win->resize(i + (qrand() % 100), i + (qrand() % 100)); - QCoreApplication::processEvents(); - } - win->close(); - }); - - // Show window - EXPECT_TRUE(runMainWindow()); - - EXPECT_TRUE(stop()); -} - -///////////////////////////////////////////////// -TEST(MinimalSceneTest, Config) -{ - setVerbosity(4); - EXPECT_TRUE(initApp()); - - // Load plugin - const char *pluginStr = - "" - "ogre" - "banana" - "1.0 0 0" - "0 1 0" - "1 2 3 0 0 1.57" - ""; - - tinyxml2::XMLDocument pluginDoc; - pluginDoc.Parse(pluginStr); - EXPECT_TRUE(ignition::gui::loadPlugin("MinimalScene", - pluginDoc.FirstChildElement("plugin"))); - - // Create main window - EXPECT_TRUE(createMainWindow()); - - // Check scene - auto engine = rendering::engine("ogre"); - ASSERT_NE(nullptr, engine); - - auto scene = engine->SceneByName("banana"); - ASSERT_NE(nullptr, scene); - - EXPECT_EQ(math::Color(0, 1, 0), scene->BackgroundColor()); - EXPECT_EQ(math::Color(1, 0, 0), scene->AmbientLight()); - - auto root = scene->RootVisual(); - ASSERT_NE(nullptr, root); - EXPECT_EQ(1u, root->ChildCount()); - - // Check camera - auto camera = std::dynamic_pointer_cast( - root->ChildByIndex(0)); - ASSERT_NE(nullptr, camera); - - EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1.57), camera->WorldPose()); - - EXPECT_TRUE(stop()); -} - diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.cc b/src/plugins/transport_scene_manager/TransportSceneManager.cc index 1f81e6e0a..b3b95e5f3 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.cc +++ b/src/plugins/transport_scene_manager/TransportSceneManager.cc @@ -17,21 +17,16 @@ #include -#include #include #include #include #include #include -#include -#include #include - -#include - -#include +#include #include +#include // TODO(louise) Remove these pragmas once ign-rendering and ign-msgs // are disabling the warnings @@ -40,9 +35,7 @@ #endif #include -#include -#include -#include +#include #include #include #include @@ -144,7 +137,7 @@ class ignition::gui::plugins::TransportSceneManagerPrivate public: std::string sceneTopic; //// \brief Pointer to the rendering scene - public: rendering::ScenePtr scene; + public: rendering::ScenePtr scene{nullptr}; //// \brief Mutex to protect the msgs public: std::mutex msgMutex; @@ -403,7 +396,6 @@ void TransportSceneManagerPrivate::OnRender() this->poses.clear(); } - ///////////////////////////////////////////////// void TransportSceneManagerPrivate::OnSceneMsg(const msgs::Scene &_msg) { diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.hh b/src/plugins/transport_scene_manager/TransportSceneManager.hh index 2d7869964..c9fd7198f 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.hh +++ b/src/plugins/transport_scene_manager/TransportSceneManager.hh @@ -30,7 +30,16 @@ namespace plugins { class TransportSceneManagerPrivate; - /// \brief + /// \brief Provides an Ignition Transport interface to + /// `ignition::gui::plugins::MinimalScene`. + /// + /// ## Configuration + /// + /// * \ : Name of service where this system will request a scene + /// message. + /// * \ : Name of topic to subscribe to receive pose updates. + /// * \ : Name of topic to request entity deletions. + /// * \ : Name of topic to receive scene updates. class TransportSceneManager : public Plugin { Q_OBJECT diff --git a/test/integration/minimal_scene.cc b/test/integration/minimal_scene.cc index 897bf28f5..fc291050f 100644 --- a/test/integration/minimal_scene.cc +++ b/test/integration/minimal_scene.cc @@ -61,6 +61,8 @@ TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) EXPECT_EQ(plugin->Title(), "3D Scene"); // Cleanup + auto pluginName = plugin->CardItem()->objectName().toStdString(); + EXPECT_TRUE(app.RemovePlugin(pluginName)); plugins.clear(); } @@ -124,5 +126,17 @@ TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) ASSERT_NE(nullptr, camera); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1.57), camera->WorldPose()); + + // Cleanup + auto plugins = win->findChildren(); + EXPECT_EQ(1, plugins.size()); + + auto pluginName = plugins[0]->CardItem()->objectName().toStdString(); + EXPECT_TRUE(app.RemovePlugin(pluginName)); + plugins.clear(); + + win->QuickWindow()->close(); + engine->DestroyScene(scene); + EXPECT_TRUE(rendering::unloadEngine(engine->Name())); } diff --git a/test/integration/transport_scene_manager.cc b/test/integration/transport_scene_manager.cc index 47bb25104..aa52509cc 100644 --- a/test/integration/transport_scene_manager.cc +++ b/test/integration/transport_scene_manager.cc @@ -68,6 +68,11 @@ TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) EXPECT_EQ(plugin->Title(), "Transport Scene Manager"); // Cleanup + for (const auto &p : plugins) + { + auto pluginName = p->CardItem()->objectName().toStdString(); + EXPECT_TRUE(app.RemovePlugin(pluginName)); + } plugins.clear(); } @@ -229,5 +234,20 @@ TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) sleep++; } EXPECT_EQ(1u, root->ChildCount()); + + // Cleanup + auto plugins = win->findChildren(); + EXPECT_EQ(plugins.size(), 2); + + for (const auto &p : plugins) + { + auto pluginName = p->CardItem()->objectName().toStdString(); + EXPECT_TRUE(app.RemovePlugin(pluginName)); + } + plugins.clear(); + + win->QuickWindow()->close(); + engine->DestroyScene(scene); + EXPECT_TRUE(rendering::unloadEngine(engine->Name())); } From 42f29c02ba2b6f7882009b5bf47a99004d0e23f6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 25 Jun 2021 14:45:44 -0700 Subject: [PATCH 16/23] PR feedback Signed-off-by: Louise Poubel --- src/plugins/minimal_scene/MinimalScene.cc | 1 - src/plugins/minimal_scene/MinimalScene.hh | 1 - src/plugins/transport_scene_manager/TransportSceneManager.cc | 1 - 3 files changed, 3 deletions(-) diff --git a/src/plugins/minimal_scene/MinimalScene.cc b/src/plugins/minimal_scene/MinimalScene.cc index a614947f7..5d4756582 100644 --- a/src/plugins/minimal_scene/MinimalScene.cc +++ b/src/plugins/minimal_scene/MinimalScene.cc @@ -128,7 +128,6 @@ IgnRenderer::IgnRenderer() { } - ///////////////////////////////////////////////// IgnRenderer::~IgnRenderer() { diff --git a/src/plugins/minimal_scene/MinimalScene.hh b/src/plugins/minimal_scene/MinimalScene.hh index 95935baad..1fdb82e49 100644 --- a/src/plugins/minimal_scene/MinimalScene.hh +++ b/src/plugins/minimal_scene/MinimalScene.hh @@ -234,7 +234,6 @@ namespace plugins public: IgnRenderer ignRenderer; }; - /// \brief A QQUickItem that manages the render window class RenderWindowItem : public QQuickItem { diff --git a/src/plugins/transport_scene_manager/TransportSceneManager.cc b/src/plugins/transport_scene_manager/TransportSceneManager.cc index b3b95e5f3..b8249c70c 100644 --- a/src/plugins/transport_scene_manager/TransportSceneManager.cc +++ b/src/plugins/transport_scene_manager/TransportSceneManager.cc @@ -15,7 +15,6 @@ * */ - #include #include #include From d68ffc874f84c082fb0ba63e95602f6da503c2f5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Jun 2021 18:15:18 +0200 Subject: [PATCH 17/23] Added feddback Signed-off-by: ahcorde --- CMakeLists.txt | 2 +- src/plugins/CMakeLists.txt | 2 +- .../CameraControllerManager.qrc | 5 - .../CMakeLists.txt | 8 +- .../CameraTracking.cc} | 112 ++++++++++++------ .../CameraTracking.hh} | 10 +- .../CameraTracking.qml} | 0 .../camera_tracking/CameraTracking.qrc | 5 + 8 files changed, 95 insertions(+), 49 deletions(-) delete mode 100644 src/plugins/camera_controller_manager/CameraControllerManager.qrc rename src/plugins/{camera_controller_manager => camera_tracking}/CMakeLists.txt (56%) rename src/plugins/{camera_controller_manager/CameraControllerManager.cc => camera_tracking/CameraTracking.cc} (77%) rename src/plugins/{camera_controller_manager/CameraControllerManager.hh => camera_tracking/CameraTracking.hh} (83%) rename src/plugins/{camera_controller_manager/CameraControllerManager.qml => camera_tracking/CameraTracking.qml} (100%) create mode 100644 src/plugins/camera_tracking/CameraTracking.qrc diff --git a/CMakeLists.txt b/CMakeLists.txt index 5539c04bd..dbc00198a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,7 @@ set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-common -ign_find_package(ignition-common4 REQUIRED VERSION 4.1) +ign_find_package(ignition-common4 REQUIRED COMPONENTS profiler VERSION 4.1) set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 16bd4f96e..68464ff2f 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -114,7 +114,7 @@ function(ign_gui_add_plugin plugin_name) endfunction() # Plugins -add_subdirectory(camera_controller_manager) +add_subdirectory(camera_tracking) add_subdirectory(grid_3d) add_subdirectory(grid_config) add_subdirectory(image_display) diff --git a/src/plugins/camera_controller_manager/CameraControllerManager.qrc b/src/plugins/camera_controller_manager/CameraControllerManager.qrc deleted file mode 100644 index 1c8d7a7a7..000000000 --- a/src/plugins/camera_controller_manager/CameraControllerManager.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - CameraControllerManager.qml - - diff --git a/src/plugins/camera_controller_manager/CMakeLists.txt b/src/plugins/camera_tracking/CMakeLists.txt similarity index 56% rename from src/plugins/camera_controller_manager/CMakeLists.txt rename to src/plugins/camera_tracking/CMakeLists.txt index bf7f1f2a3..f235b22f3 100644 --- a/src/plugins/camera_controller_manager/CMakeLists.txt +++ b/src/plugins/camera_tracking/CMakeLists.txt @@ -1,10 +1,10 @@ -ign_gui_add_plugin(CameraControllerManager +ign_gui_add_plugin(CameraTracking SOURCES - CameraControllerManager.cc + CameraTracking.cc QT_HEADERS - CameraControllerManager.hh + CameraTracking.hh TEST_SOURCES - # CameraControllerManager_TEST.cc + # CameraTracking_TEST.cc PUBLIC_LINK_LIBS ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ignition-common${IGN_COMMON_VER}::profiler diff --git a/src/plugins/camera_controller_manager/CameraControllerManager.cc b/src/plugins/camera_tracking/CameraTracking.cc similarity index 77% rename from src/plugins/camera_controller_manager/CameraControllerManager.cc rename to src/plugins/camera_tracking/CameraTracking.cc index 23e85a429..81833bf99 100644 --- a/src/plugins/camera_controller_manager/CameraControllerManager.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -26,7 +26,8 @@ #ifdef _MSC_VER #pragma warning(push, 0) #endif -#include +#include +#include #include #include @@ -44,10 +45,10 @@ #include -#include "CameraControllerManager.hh" +#include "CameraTracking.hh" -/// \brief Private data class for CameraControllerManager -class ignition::gui::plugins::CameraControllerManagerPrivate +/// \brief Private data class for CameraTracking +class ignition::gui::plugins::CameraTrackingPrivate { public: void OnRender(); @@ -74,13 +75,20 @@ class ignition::gui::plugins::CameraControllerManagerPrivate public: bool OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res); + /// \brief Callback for a follow offset request + /// \param[in] _msg Request message to set the camera's follow offset. + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnFollowOffset(const msgs::Vector3d &_msg, + msgs::Boolean &_res); + /// \brief Callback when a move to animation is complete private: void OnMoveToComplete(); /// \brief Callback when a move to pose animation is complete private: void OnMoveToPoseComplete(); - public: void HandleKeyRelease(events::KeyReleaseToScene *_e); + public: void HandleKeyRelease(events::KeyReleaseOnScene *_e); public: std::mutex mutex; @@ -93,12 +101,15 @@ class ignition::gui::plugins::CameraControllerManagerPrivate /// \brief Wait for follow target public: bool followTargetWait = false; - /// \brief Offset of camera from taget being followed + /// \brief Offset of camera from target being followed public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3); /// \brief Flag to indicate the follow offset needs to be updated public: bool followOffsetDirty = false; + /// \brief Flag to indicate the follow offset has been updated + public: bool newFollowOffset = true; + /// \brief Follow P gain public: double followPGain = 0.01; @@ -130,6 +141,9 @@ class ignition::gui::plugins::CameraControllerManagerPrivate /// \brief Follow service public: std::string followService; + /// \brief Follow offset service + public: std::string followOffsetService; + /// \brief Camera pose topic public: std::string cameraPoseTopic; @@ -148,9 +162,20 @@ using namespace gui; using namespace plugins; ///////////////////////////////////////////////// -void CameraControllerManagerPrivate::Initialize() +void CameraTrackingPrivate::Initialize() { - this->camera = std::dynamic_pointer_cast(this->scene->SensorByName("Scene3DCamera")); + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam) + { + this->camera = cam; + igndbg << "CameraTrackingPrivate plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } if (!this->camera) { ignerr << "Camera is not available" << std::endl; @@ -161,14 +186,14 @@ void CameraControllerManagerPrivate::Initialize() // move to this->moveToService = "/gui/move_to"; this->node.Advertise(this->moveToService, - &CameraControllerManagerPrivate::OnMoveTo, this); + &CameraTrackingPrivate::OnMoveTo, this); ignmsg << "Move to service on [" << this->moveToService << "]" << std::endl; // follow this->followService = "/gui/follow"; this->node.Advertise(this->followService, - &CameraControllerManagerPrivate::OnFollow, this); + &CameraTrackingPrivate::OnFollow, this); ignmsg << "Follow service on [" << this->followService << "]" << std::endl; @@ -176,7 +201,7 @@ void CameraControllerManagerPrivate::Initialize() this->moveToPoseService = "/gui/move_to/pose"; this->node.Advertise(this->moveToPoseService, - &CameraControllerManagerPrivate::OnMoveToPose, this); + &CameraTrackingPrivate::OnMoveToPose, this); ignmsg << "Move to pose service on [" << this->moveToPoseService << "]" << std::endl; @@ -186,10 +211,17 @@ void CameraControllerManagerPrivate::Initialize() this->node.Advertise(this->cameraPoseTopic); ignmsg << "Camera pose topic advertised on [" << this->cameraPoseTopic << "]" << std::endl; + + // follow offset + this->followOffsetService = "/gui/follow/offset"; + this->node.Advertise(this->followOffsetService, + &CameraTrackingPrivate::OnFollowOffset, this); + ignmsg << "Follow offset service on [" + << this->followOffsetService << "]" << std::endl; } ///////////////////////////////////////////////// -bool CameraControllerManagerPrivate::OnMoveTo(const msgs::StringMsg &_msg, +bool CameraTrackingPrivate::OnMoveTo(const msgs::StringMsg &_msg, msgs::Boolean &_res) { this->moveToTarget = _msg.data(); @@ -199,7 +231,7 @@ bool CameraControllerManagerPrivate::OnMoveTo(const msgs::StringMsg &_msg, } ///////////////////////////////////////////////// -bool CameraControllerManagerPrivate::OnFollow(const msgs::StringMsg &_msg, +bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg, msgs::Boolean &_res) { this->followTarget = _msg.data(); @@ -209,21 +241,33 @@ bool CameraControllerManagerPrivate::OnFollow(const msgs::StringMsg &_msg, } ///////////////////////////////////////////////// -void CameraControllerManagerPrivate::OnMoveToComplete() +void CameraTrackingPrivate::OnMoveToComplete() { std::lock_guard lock(this->mutex); this->moveToTarget.clear(); } ///////////////////////////////////////////////// -void CameraControllerManagerPrivate::OnMoveToPoseComplete() +void CameraTrackingPrivate::OnMoveToPoseComplete() { std::lock_guard lock(this->mutex); this->moveToPoseValue.reset(); } ///////////////////////////////////////////////// -bool CameraControllerManagerPrivate::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) +bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boolean &_res) +{ + math::Vector3d offset = msgs::Convert(_msg); + + if (!this->followTarget.empty()) + this->newFollowOffset = true; + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) { math::Pose3d pose = msgs::Convert(_msg.pose()); @@ -248,7 +292,7 @@ bool CameraControllerManagerPrivate::OnMoveToPose(const msgs::GUICamera &_msg, m } ///////////////////////////////////////////////// -void CameraControllerManagerPrivate::OnRender() +void CameraTrackingPrivate::OnRender() { if (nullptr == this->scene) { @@ -261,7 +305,7 @@ void CameraControllerManagerPrivate::OnRender() // Move To { - IGN_PROFILE("CameraControllerManagerPrivate::OnRender MoveTo"); + IGN_PROFILE("CameraTrackingPrivate::OnRender MoveTo"); if (!this->moveToTarget.empty()) { if (this->moveToHelper.Idle()) @@ -271,7 +315,7 @@ void CameraControllerManagerPrivate::OnRender() if (target) { this->moveToHelper.MoveTo(this->camera, target, 0.5, - std::bind(&CameraControllerManagerPrivate::OnMoveToComplete, this)); + std::bind(&CameraTrackingPrivate::OnMoveToComplete, this)); this->prevMoveToTime = std::chrono::system_clock::now(); } else @@ -293,14 +337,14 @@ void CameraControllerManagerPrivate::OnRender() // Move to pose { - IGN_PROFILE("CameraControllerManagerPrivate::OnRender MoveToPose"); + IGN_PROFILE("CameraTrackingPrivate::OnRender MoveToPose"); if (this->moveToPoseValue) { if (this->moveToHelper.Idle()) { this->moveToHelper.MoveTo(this->camera, *(this->moveToPoseValue), - 0.5, std::bind(&CameraControllerManagerPrivate::OnMoveToPoseComplete, this)); + 0.5, std::bind(&CameraTrackingPrivate::OnMoveToPoseComplete, this)); this->prevMoveToTime = std::chrono::system_clock::now(); } else @@ -315,7 +359,7 @@ void CameraControllerManagerPrivate::OnRender() // Follow { - IGN_PROFILE("CameraControllerManagerPrivate::OnRender Follow"); + IGN_PROFILE("CameraTrackingPrivate::OnRender Follow"); // reset follow mode if target node got removed if (!this->followTarget.empty()) { @@ -337,7 +381,8 @@ void CameraControllerManagerPrivate::OnRender() this->followTarget); if (target) { - if (!followTargetTmp || target != followTargetTmp) + if (!followTargetTmp || target != followTargetTmp + || this->newFollowOffset) { this->camera->SetFollowTarget(target, this->followOffset, @@ -346,6 +391,7 @@ void CameraControllerManagerPrivate::OnRender() this->camera->SetTrackTarget(target); // found target, no need to wait anymore + this->newFollowOffset = false; this->followTargetWait = false; } else if (this->followOffsetDirty) @@ -376,8 +422,8 @@ void CameraControllerManagerPrivate::OnRender() } ///////////////////////////////////////////////// -CameraControllerManager::CameraControllerManager() - : Plugin(), dataPtr(new CameraControllerManagerPrivate) +CameraTracking::CameraTracking() + : Plugin(), dataPtr(new CameraTrackingPrivate) { this->dataPtr->timer = new QTimer(this); this->connect(this->dataPtr->timer, &QTimer::timeout, [=]() @@ -395,12 +441,12 @@ CameraControllerManager::CameraControllerManager() } ///////////////////////////////////////////////// -CameraControllerManager::~CameraControllerManager() +CameraTracking::~CameraTracking() { } ///////////////////////////////////////////////// -void CameraControllerManager::LoadConfig(const tinyxml2::XMLElement *) +void CameraTracking::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) this->title = "Camera Controller Manager"; @@ -409,9 +455,9 @@ void CameraControllerManager::LoadConfig(const tinyxml2::XMLElement *) } ///////////////////////////////////////////////// -void CameraControllerManagerPrivate::HandleKeyRelease(events::KeyReleaseToScene *_e) +void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e) { - if (_e->Key() == Qt::Key_Escape) + if (_e->Key().Key() == Qt::Key_Escape) { if (!this->followTarget.empty()) { @@ -423,15 +469,15 @@ void CameraControllerManagerPrivate::HandleKeyRelease(events::KeyReleaseToScene } ///////////////////////////////////////////////// -bool CameraControllerManager::eventFilter(QObject *_obj, QEvent *_event) +bool CameraTracking::eventFilter(QObject *_obj, QEvent *_event) { if (_event->type() == events::Render::kType) { this->dataPtr->OnRender(); } - else if (_event->type() == events::KeyReleaseToScene::kType) + else if (_event->type() == events::KeyReleaseOnScene::kType) { - events::KeyReleaseToScene *keyEvent = static_cast(_event); + events::KeyReleaseOnScene *keyEvent = static_cast(_event); if (keyEvent) { this->dataPtr->HandleKeyRelease(keyEvent); @@ -442,5 +488,5 @@ bool CameraControllerManager::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gui::plugins::CameraControllerManager, +IGNITION_ADD_PLUGIN(ignition::gui::plugins::CameraTracking, ignition::gui::Plugin) diff --git a/src/plugins/camera_controller_manager/CameraControllerManager.hh b/src/plugins/camera_tracking/CameraTracking.hh similarity index 83% rename from src/plugins/camera_controller_manager/CameraControllerManager.hh rename to src/plugins/camera_tracking/CameraTracking.hh index 0846cb518..466488755 100644 --- a/src/plugins/camera_controller_manager/CameraControllerManager.hh +++ b/src/plugins/camera_tracking/CameraTracking.hh @@ -28,17 +28,17 @@ namespace gui { namespace plugins { - class CameraControllerManagerPrivate; + class CameraTrackingPrivate; - class CameraControllerManager : public Plugin + class CameraTracking : public Plugin { Q_OBJECT /// \brief Constructor - public: CameraControllerManager(); + public: CameraTracking(); /// \brief Destructor - public: virtual ~CameraControllerManager(); + public: virtual ~CameraTracking(); // Documentation inherited public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) @@ -49,7 +49,7 @@ namespace plugins /// \internal /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; }; } } diff --git a/src/plugins/camera_controller_manager/CameraControllerManager.qml b/src/plugins/camera_tracking/CameraTracking.qml similarity index 100% rename from src/plugins/camera_controller_manager/CameraControllerManager.qml rename to src/plugins/camera_tracking/CameraTracking.qml diff --git a/src/plugins/camera_tracking/CameraTracking.qrc b/src/plugins/camera_tracking/CameraTracking.qrc new file mode 100644 index 000000000..f58398257 --- /dev/null +++ b/src/plugins/camera_tracking/CameraTracking.qrc @@ -0,0 +1,5 @@ + + + CameraTracking.qml + + From 65fb2349764a2104bf40a748c325f76ba31d6f21 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Jun 2021 18:25:09 +0200 Subject: [PATCH 18/23] make linters happy Signed-off-by: ahcorde --- src/plugins/camera_tracking/CameraTracking.cc | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index 81833bf99..2378e9237 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -255,7 +256,8 @@ void CameraTrackingPrivate::OnMoveToPoseComplete() } ///////////////////////////////////////////////// -bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boolean &_res) +bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, + msgs::Boolean &_res) { math::Vector3d offset = msgs::Convert(_msg); @@ -267,7 +269,8 @@ bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boo } ///////////////////////////////////////////////// -bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) +bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg, + msgs::Boolean &_res) { math::Pose3d pose = msgs::Convert(_msg.pose()); @@ -477,7 +480,8 @@ bool CameraTracking::eventFilter(QObject *_obj, QEvent *_event) } else if (_event->type() == events::KeyReleaseOnScene::kType) { - events::KeyReleaseOnScene *keyEvent = static_cast(_event); + events::KeyReleaseOnScene *keyEvent = + static_cast(_event); if (keyEvent) { this->dataPtr->HandleKeyRelease(keyEvent); From cdb85dcd236ad7d1ff696c9f29406891647b6aa5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 30 Jun 2021 16:56:47 -0700 Subject: [PATCH 19/23] ign-utils impl Signed-off-by: Louise Poubel --- src/plugins/minimal_scene/MinimalScene.cc | 112 +++++++++------------- src/plugins/minimal_scene/MinimalScene.hh | 20 +--- 2 files changed, 48 insertions(+), 84 deletions(-) diff --git a/src/plugins/minimal_scene/MinimalScene.cc b/src/plugins/minimal_scene/MinimalScene.cc index 5d4756582..6c21c5716 100644 --- a/src/plugins/minimal_scene/MinimalScene.cc +++ b/src/plugins/minimal_scene/MinimalScene.cc @@ -52,84 +52,70 @@ #include "ignition/gui/GuiEvents.hh" #include "ignition/gui/MainWindow.hh" -namespace ignition +/// \brief Private data class for IgnRenderer +class ignition::gui::plugins::IgnRenderer::Implementation { -namespace gui -{ -namespace plugins -{ - /// \brief Private data class for IgnRenderer - class IgnRendererPrivate - { - /// \brief Flag to indicate if mouse event is dirty - public: bool mouseDirty = false; + /// \brief Flag to indicate if mouse event is dirty + public: bool mouseDirty{false}; - /// \brief Flag to indicate if hover event is dirty - public: bool hoverDirty = false; + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty{false}; - /// \brief Mouse event - public: common::MouseEvent mouseEvent; + /// \brief Mouse event + public: common::MouseEvent mouseEvent; - /// \brief Key event - public: common::KeyEvent keyEvent; + /// \brief Key event + public: common::KeyEvent keyEvent; - /// \brief Mouse move distance since last event. - public: math::Vector2d drag; + /// \brief Mouse move distance since last event. + public: math::Vector2d drag; - /// \brief Mutex to protect mouse events - public: std::mutex mutex; + /// \brief Mutex to protect mouse events + public: std::mutex mutex; - /// \brief User camera - public: rendering::CameraPtr camera; + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; - /// \brief Camera orbit controller - public: rendering::OrbitViewController viewControl; + /// \brief Camera orbit controller + public: rendering::OrbitViewController viewControl; - /// \brief The currently hovered mouse position in screen coordinates - public: math::Vector2i mouseHoverPos{math::Vector2i::Zero}; + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos{math::Vector2i::Zero}; - /// \brief Ray query for mouse clicks - public: rendering::RayQueryPtr rayQuery{nullptr}; + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery{nullptr}; - /// \brief View control focus target - public: math::Vector3d target; - }; + /// \brief View control focus target + public: math::Vector3d target; +}; - /// \brief Private data class for RenderWindowItem - class RenderWindowItemPrivate - { - /// \brief Keep latest mouse event - public: common::MouseEvent mouseEvent; +/// \brief Private data class for RenderWindowItem +class ignition::gui::plugins::RenderWindowItem::Implementation +{ + /// \brief Keep latest mouse event + public: common::MouseEvent mouseEvent; - /// \brief Render thread - public : RenderThread *renderThread = nullptr; + /// \brief Render thread + public : RenderThread *renderThread = nullptr; - //// \brief List of threads - public: static QList threads; - }; + //// \brief List of threads + public: static QList threads; +}; - /// \brief Private data class for MinimalScene - class MinimalScenePrivate - { - }; -} -} -} +/// \brief Private data class for MinimalScene +class ignition::gui::plugins::MinimalScene::Implementation +{ +}; using namespace ignition; using namespace gui; using namespace plugins; -QList RenderWindowItemPrivate::threads; +QList RenderWindowItem::Implementation::threads; ///////////////////////////////////////////////// IgnRenderer::IgnRenderer() - : dataPtr(new IgnRendererPrivate) -{ -} - -///////////////////////////////////////////////// -IgnRenderer::~IgnRenderer() + : dataPtr(utils::MakeUniqueImpl()) { } @@ -448,7 +434,7 @@ math::Vector3d IgnRenderer::ScreenToScene( ///////////////////////////////////////////////// RenderThread::RenderThread() { - RenderWindowItemPrivate::threads << this; + RenderWindowItem::Implementation::threads << this; } ///////////////////////////////////////////////// @@ -585,18 +571,13 @@ void TextureNode::PrepareNode() ///////////////////////////////////////////////// RenderWindowItem::RenderWindowItem(QQuickItem *_parent) - : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) + : QQuickItem(_parent), dataPtr(utils::MakeUniqueImpl()) { this->setAcceptedMouseButtons(Qt::AllButtons); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); } -///////////////////////////////////////////////// -RenderWindowItem::~RenderWindowItem() -{ -} - ///////////////////////////////////////////////// void RenderWindowItem::Ready() { @@ -752,16 +733,11 @@ void RenderWindowItem::SetSkyEnabled(const bool &_sky) ///////////////////////////////////////////////// MinimalScene::MinimalScene() - : Plugin(), dataPtr(new MinimalScenePrivate) + : Plugin(), dataPtr(utils::MakeUniqueImpl()) { qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); } -///////////////////////////////////////////////// -MinimalScene::~MinimalScene() -{ -} - ///////////////////////////////////////////////// void MinimalScene::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { diff --git a/src/plugins/minimal_scene/MinimalScene.hh b/src/plugins/minimal_scene/MinimalScene.hh index 1fdb82e49..c2249f078 100644 --- a/src/plugins/minimal_scene/MinimalScene.hh +++ b/src/plugins/minimal_scene/MinimalScene.hh @@ -25,6 +25,7 @@ #include #include #include +#include #include "ignition/gui/Plugin.hh" @@ -34,10 +35,6 @@ namespace gui { namespace plugins { - class IgnRendererPrivate; - class RenderWindowItemPrivate; - class MinimalScenePrivate; - /// \brief Creates a new ignition rendering scene or adds a user-camera to an /// existing scene. It is possible to orbit the camera around the scene with /// the mouse. Use other plugins to manage objects in the scene. @@ -62,9 +59,6 @@ namespace plugins /// \brief Constructor public: MinimalScene(); - /// \brief Destructor - public: virtual ~MinimalScene(); - /// \brief Callback when the mouse hovers to a new position. /// \param[in] _mouseX x coordinate of the hovered mouse position. /// \param[in] _mouseY y coordinate of the hovered mouse position. @@ -83,7 +77,7 @@ namespace plugins /// \internal /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; /// \brief Ign-rendering renderer. @@ -97,9 +91,6 @@ namespace plugins /// \brief Constructor public: IgnRenderer(); - /// \brief Destructor - public: ~IgnRenderer(); - /// \brief Main render function public: void Render(); @@ -198,7 +189,7 @@ namespace plugins /// \internal /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; /// \brief Rendering thread @@ -243,9 +234,6 @@ namespace plugins /// \param[in] _parent Parent item public: explicit RenderWindowItem(QQuickItem *_parent = nullptr); - /// \brief Destructor - public: virtual ~RenderWindowItem(); - /// \brief Set background color of render window /// \param[in] _color Color of render window background public: void SetBackgroundColor(const math::Color &_color); @@ -332,7 +320,7 @@ namespace plugins /// \internal /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; /// \brief Texture node for displaying the render texture from ign-renderer From 5fc8a019604917ad8a242de6e386ab915c568c1a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 30 Jun 2021 19:13:57 -0700 Subject: [PATCH 20/23] Add to scene.config and to examples Signed-off-by: Louise Poubel --- examples/config/scene3d.config | 13 +++++++++ examples/standalone/scene_provider/README.md | 29 +++++++++++++++++++ src/plugins/camera_tracking/CameraTracking.cc | 4 ++- 3 files changed, 45 insertions(+), 1 deletion(-) diff --git a/examples/config/scene3d.config b/examples/config/scene3d.config index 537a8e3c0..1f6caa286 100644 --- a/examples/config/scene3d.config +++ b/examples/config/scene3d.config @@ -32,6 +32,19 @@ /example/delete /example/scene + + + + + + + false + 5 + 5 + floating + false + + Controls diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md index 676f9614d..d60425399 100644 --- a/examples/standalone/scene_provider/README.md +++ b/examples/standalone/scene_provider/README.md @@ -34,3 +34,32 @@ ign gui -c examples/config/scene3d.config You should see a black box moving around the scene. +## Testing other plugins + +### Camera tracking + +Some commands to test camera tracking with this demo: + +Move to box: + +``` +ign service -s /gui/move_to --reqtype ignition.msgs.StringMsg --reptype ignition.msgs.Boolean --timeout 2000 --req 'data: "box_model"' +``` + +Echo camera pose: + +``` +ign topic -e -t /gui/camera/pose +``` + +Follow box: + +``` +ign service -s /gui/follow --reqtype ignition.msgs.StringMsg --reptype ignition.msgs.Boolean --timeout 2000 --req 'data: "box_model"' +``` + +Update follow offset: + +``` +ign service -s /gui/follow/offset --reqtype ignition.msgs.Vector3d --reptype ignition.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5' +``` diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index 2378e9237..e4de57edd 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -182,7 +182,6 @@ void CameraTrackingPrivate::Initialize() ignerr << "Camera is not available" << std::endl; return; } - ignerr << "Camera is available!!" << std::endl; // move to this->moveToService = "/gui/move_to"; @@ -262,7 +261,10 @@ bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, math::Vector3d offset = msgs::Convert(_msg); if (!this->followTarget.empty()) + { this->newFollowOffset = true; + this->followOffset = msgs::Convert(_msg); + } _res.set_data(true); return true; From 1a75774a6ad8b3d267e9e25c6cdfd4c7f46b8bf5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Jul 2021 13:01:03 +0200 Subject: [PATCH 21/23] Added feedback and other tweaks Signed-off-by: ahcorde --- src/plugins/camera_tracking/CameraTracking.cc | 13 +++++++------ src/plugins/camera_tracking/CameraTracking.hh | 4 ++-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index e4de57edd..c0c9331d6 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -171,10 +171,13 @@ void CameraTrackingPrivate::Initialize() scene->NodeByIndex(i)); if (cam) { - this->camera = cam; - igndbg << "CameraTrackingPrivate plugin is moving camera [" - << this->camera->Name() << "]" << std::endl; - break; + if (cam->Name().find("scene::Camera") != std::string::npos) + { + this->camera = cam; + igndbg << "CameraTrackingPrivate plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; + } } } if (!this->camera) @@ -258,8 +261,6 @@ void CameraTrackingPrivate::OnMoveToPoseComplete() bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boolean &_res) { - math::Vector3d offset = msgs::Convert(_msg); - if (!this->followTarget.empty()) { this->newFollowOffset = true; diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh index 466488755..f43405bbc 100644 --- a/src/plugins/camera_tracking/CameraTracking.hh +++ b/src/plugins/camera_tracking/CameraTracking.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ -#define IGNITION_GUI_PLUGINS_CAMERACONTROLLERMANAGER_HH_ +#ifndef IGNITION_GUI_PLUGINS_CAMERATRACKING_HH_ +#define IGNITION_GUI_PLUGINS_CAMERATRACKING_HH_ #include From cb656ee0dcee09647bcfc607e15312882bf053bf Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Jul 2021 20:35:40 +0200 Subject: [PATCH 22/23] Restored Scene3d example config file Signed-off-by: ahcorde --- examples/config/scene3d.config | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/examples/config/scene3d.config b/examples/config/scene3d.config index 537a8e3c0..1f6caa286 100644 --- a/examples/config/scene3d.config +++ b/examples/config/scene3d.config @@ -32,6 +32,19 @@ /example/delete /example/scene + + + + + + + false + 5 + 5 + floating + false + + Controls From 3a5ae7546e13ccc9432c732544c69736af274e6f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 1 Jul 2021 19:41:02 -0700 Subject: [PATCH 23/23] Add test, some cleanup Signed-off-by: Louise Poubel --- src/plugins/camera_tracking/CameraTracking.cc | 32 ++- test/integration/camera_tracking.cc | 205 ++++++++++++++++++ 2 files changed, 226 insertions(+), 11 deletions(-) create mode 100644 test/integration/camera_tracking.cc diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index c0c9331d6..7a01746af 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -51,8 +51,10 @@ /// \brief Private data class for CameraTracking class ignition::gui::plugins::CameraTrackingPrivate { + /// \brief Perform rendering calls in the rendering thread. public: void OnRender(); + /// \brief Initialize transport public: void Initialize(); /// \brief Callback for a move to request @@ -89,8 +91,11 @@ class ignition::gui::plugins::CameraTrackingPrivate /// \brief Callback when a move to pose animation is complete private: void OnMoveToPoseComplete(); + /// \brief Process key releases + /// \param[in] _e Key release event public: void HandleKeyRelease(events::KeyReleaseOnScene *_e); + /// \brief Protects variable changed through services. public: std::mutex mutex; //// \brief Pointer to the rendering scene @@ -122,7 +127,7 @@ class ignition::gui::plugins::CameraTrackingPrivate public: std::chrono::time_point prevMoveToTime; /// \brief User camera - public: rendering::CameraPtr camera; + public: rendering::CameraPtr camera{nullptr}; /// \brief Target to move the user camera to public: std::string moveToTarget; @@ -154,8 +159,8 @@ class ignition::gui::plugins::CameraTrackingPrivate /// \brief Camera pose publisher public: transport::Node::Publisher cameraPosePub; - /// \brief Timer to keep publishing - public: QTimer *timer; + /// \brief Timer to keep publishing camera poses. + public: QTimer *timer{nullptr}; }; using namespace ignition; @@ -165,19 +170,17 @@ using namespace plugins; ///////////////////////////////////////////////// void CameraTrackingPrivate::Initialize() { + // Attach to the first camera we find for (unsigned int i = 0; i < scene->NodeCount(); ++i) { auto cam = std::dynamic_pointer_cast( scene->NodeByIndex(i)); if (cam) { - if (cam->Name().find("scene::Camera") != std::string::npos) - { - this->camera = cam; - igndbg << "CameraTrackingPrivate plugin is moving camera [" - << this->camera->Name() << "]" << std::endl; - break; - } + this->camera = cam; + igndbg << "CameraTrackingPrivate plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; } } if (!this->camera) @@ -227,6 +230,7 @@ void CameraTrackingPrivate::Initialize() bool CameraTrackingPrivate::OnMoveTo(const msgs::StringMsg &_msg, msgs::Boolean &_res) { + std::lock_guard lock(this->mutex); this->moveToTarget = _msg.data(); _res.set_data(true); @@ -237,6 +241,7 @@ bool CameraTrackingPrivate::OnMoveTo(const msgs::StringMsg &_msg, bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg, msgs::Boolean &_res) { + std::lock_guard lock(this->mutex); this->followTarget = _msg.data(); _res.set_data(true); @@ -261,6 +266,7 @@ void CameraTrackingPrivate::OnMoveToPoseComplete() bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boolean &_res) { + std::lock_guard lock(this->mutex); if (!this->followTarget.empty()) { this->newFollowOffset = true; @@ -275,6 +281,7 @@ bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) { + std::lock_guard lock(this->mutex); math::Pose3d pose = msgs::Convert(_msg.pose()); // If there is no orientation in the message, then set a Rot value in the @@ -309,6 +316,9 @@ void CameraTrackingPrivate::OnRender() this->Initialize(); } + if (!this->camera) + return; + // Move To { IGN_PROFILE("CameraTrackingPrivate::OnRender MoveTo"); @@ -455,7 +465,7 @@ CameraTracking::~CameraTracking() void CameraTracking::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) - this->title = "Camera Controller Manager"; + this->title = "Camera tracking"; App()->findChild()->installEventFilter(this); } diff --git a/test/integration/camera_tracking.cc b/test/integration/camera_tracking.cc new file mode 100644 index 000000000..7b94e03d1 --- /dev/null +++ b/test/integration/camera_tracking.cc @@ -0,0 +1,205 @@ +/* + * 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 "test_config.h" // NOLINT(build/include) +#include "ignition/gui/Application.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/Plugin.hh" +#include "ignition/gui/MainWindow.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./camera_tracking")), +}; + +using namespace ignition; +using namespace gui; +using namespace std::chrono_literals; + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) +{ + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Load plugins + const char *pluginStr = + "" + "ogre" + "banana" + "1.0 0 0" + "0 1 0" + "1 2 3 0 0 0" + ""; + + tinyxml2::XMLDocument pluginDoc; + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("MinimalScene", + pluginDoc.FirstChildElement("plugin"))); + + pluginStr = + "" + ""; + + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("CameraTracking", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + EXPECT_EQ(plugins.size(), 2); + + auto plugin = plugins[0]; + EXPECT_EQ(plugin->Title(), "3D Scene"); + + plugin = plugins[1]; + EXPECT_EQ(plugin->Title(), "Camera tracking"); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Get camera pose + msgs::Pose poseMsg; + auto poseCb = std::function( + [&](const auto &_msg) + { + poseMsg = _msg; + }); + + transport::Node node; + node.Subscribe("/gui/camera/pose", poseCb); + + int sleep = 0; + int maxSleep = 30; + while (!poseMsg.has_position() && sleep++ < maxSleep) + { + std::this_thread::sleep_for(100ms); + QCoreApplication::processEvents(); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_TRUE(poseMsg.has_position()); + EXPECT_TRUE(poseMsg.has_orientation()); + + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + auto scene = engine->SceneByName("banana"); + ASSERT_NE(nullptr, scene); + + auto root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + auto camera = std::dynamic_pointer_cast( + root->ChildByIndex(0)); + ASSERT_NE(nullptr, camera); + + EXPECT_EQ(camera->WorldPose(), msgs::Convert(poseMsg)); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), msgs::Convert(poseMsg)); + + // Add object to be tracked + auto trackedVis = scene->CreateVisual("track_me"); + ASSERT_NE(nullptr, trackedVis); + trackedVis->SetWorldPose({100, 100, 100, 0, 0, 0}); + + // Move to + msgs::StringMsg req; + msgs::Boolean rep; + + req.set_data("track_me"); + + bool result; + unsigned int timeout = 2000; + bool executed = node.Request("/gui/move_to", req, timeout, rep, result); + EXPECT_TRUE(executed); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + + sleep = 0; + while (abs(camera->WorldPose().Pos().X() - 100) > 10 && sleep++ < maxSleep) + { + std::this_thread::sleep_for(100ms); + QCoreApplication::processEvents(); + } + EXPECT_LT(sleep, maxSleep); + + EXPECT_GT(10, abs(camera->WorldPose().Pos().X() - 100)); + EXPECT_GT(10, abs(camera->WorldPose().Pos().Y() - 100)); + EXPECT_GT(10, abs(camera->WorldPose().Pos().Z() - 100)); + + // Move target object to new position + trackedVis->SetWorldPose({130, 130, 130, 0, 0, 0}); + + // Follow + result = false; + executed = node.Request("/gui/follow", req, timeout, rep, result); + EXPECT_TRUE(executed); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + + msgs::Vector3d reqOffset; + reqOffset.set_x(1.0); + reqOffset.set_y(1.0); + reqOffset.set_z(1.0); + result = false; + executed = node.Request("/gui/follow/offset", reqOffset, timeout, rep, + result); + EXPECT_TRUE(executed); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + + // Many update loops to process many events + maxSleep = 300; + for (auto it : {150.0, 200.0}) + { + // Move target + trackedVis->SetWorldPose({it, it, it, 0, 0, 0}); + + // Check camera moved + sleep = 0; + while (abs(camera->WorldPose().Pos().X() - it) > 10 && + sleep++ < maxSleep) + { + std::this_thread::sleep_for(10ms); + QCoreApplication::processEvents(); + } + EXPECT_LT(sleep, maxSleep); + + EXPECT_GT(10, abs(camera->WorldPose().Pos().X() - it)); + EXPECT_GT(10, abs(camera->WorldPose().Pos().Y() - it)); + EXPECT_GT(10, abs(camera->WorldPose().Pos().Z() - it)); + } +} +