From 890cc29d9a9b323b7d7e6ce7c5357ca46264eacd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 12 Sep 2023 14:56:41 -0700 Subject: [PATCH 1/8] Prepare from 8.0.0 pre2 release (#904) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fc62a163f..8bcf7b794 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR}) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -gz_configure_project(VERSION_SUFFIX pre1) +gz_configure_project(VERSION_SUFFIX pre2) #============================================================================ # Set project-specific options From 98c5978e9f543c4d64177f6d1fa80b9ba0c777a7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Sep 2023 05:22:44 -0700 Subject: [PATCH 2/8] Update installation instructions (#906) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ian Chen Signed-off-by: Michael Carroll Co-authored-by: Michael Carroll Co-authored-by: Alejandro Hernández Cordero --- tutorials/02_install.md | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/tutorials/02_install.md b/tutorials/02_install.md index 47829748f..a5beb4118 100644 --- a/tutorials/02_install.md +++ b/tutorials/02_install.md @@ -34,7 +34,7 @@ sudo apt-get update sudo apt-get install libgz-rendering<#>-dev ``` -Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. +Be sure to replace `<#>` with a number value, such as `7` or `8`, depending on which version you need. ## Source Installation @@ -70,11 +70,11 @@ build the relevant plugins if dependencies are found. **OGRE 1.x** ``` -# this installs ogre 1.9. Alternatively, you can install 1.8 +# this installs ogre 1.9 sudo apt-get install libogre-1.9-dev ``` -**OGRE 2.x (supported in Versions >= gz-rendering1)** +**OGRE-Next 2.x** Add OSRF packages if you have not done so already: ``` @@ -84,9 +84,9 @@ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt update ``` -Install OGRE 2.2 debs +Install OGRE-Next 2.3 debs ``` -sudo apt install libogre-2.2-dev +sudo apt install libogre-next-dev ``` **OptiX (experimental)** @@ -114,7 +114,7 @@ sdk 4.0.2, comment out lines 167-206). 1. Clone the repository ``` - # Optionally, append `-b ign-rendering#` (replace # with a number) to check out a specific version + # Optionally, append `-b gz-rendering#` (replace # with a number) to check out a specific version git clone http://github.com/gazebosim/gz-rendering ``` @@ -151,7 +151,12 @@ conda activate gz-ws ## Binary Installation -`libgz-rendering<#>` Conda feedstock is not yet available, pending [conda-forge/staged-recipes#13551](https://github.com/conda-forge/staged-recipes/issues/13551). +```bash +conda install libgz-rendering<#> --channel conda-forge +``` + +Be sure to replace `<#>` with a number value, such as 7 or 8, depending on +which version you need. ## Source Installation @@ -176,7 +181,7 @@ This assumes you have created and activated a Conda environment while installing 3. Navigate to where you would like to build the library, and clone the repository. ``` - # Optionally, append `-b ign-rendering#` (replace # with a number) to check out a specific version + # Optionally, append `-b gz-rendering#` (replace # with a number) to check out a specific version git clone https://github.com/gazebosim/gz-rendering.git ``` @@ -209,23 +214,23 @@ Install Gazebo Rendering: brew install gz-rendering<#> ``` -Be sure to replace `<#>` with a number value, such as 5 or 6, depending on +Be sure to replace `<#>` with a number value, such as 7 or 8, depending on which version you need. ## Source Installation 1. Clone the repository ``` - git clone https://github.com/gazebosim/gz-rendering -b ign-rendering<#> + git clone https://github.com/gazebosim/gz-rendering -b gz-rendering<#> ``` - Be sure to replace `<#>` with a number value, such as 5 or 6, depending on + Be sure to replace `<#>` with a number value, such as 7 or 8, depending on which version you need. 2. Install dependencies ``` brew install --only-dependencies gz-rendering<#> ``` - Be sure to replace `<#>` with a number value, such as 5 or 6, depending on + Be sure to replace `<#>` with a number value, such as 7 or 8, depending on which version you need. 3. Configure and build @@ -273,7 +278,7 @@ To control the testing configuration, use the following environment variables: ``` # Specify the rendering engine to use (ogre, ogre2, optix) - GZ_ENGINE_TO_TEST=ogre2 + GZ_ENGINE_TO_TEST=ogre2 # Specify the ogre2 backend to use (vulkan, gl3plus, metal (macOS)) GZ_ENGINE_BACKEND=vulkan From 0795e095914a4c7abd805f9272153d3caa98d4f4 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 14 Sep 2023 21:12:42 +0200 Subject: [PATCH 3/8] Fixed build on MSVC. (#897) Signed-off-by: Martin Pecka Co-authored-by: Ian Chen Co-authored-by: Michael Carroll --- .../gz/rendering/GlobalIlluminationBase.hh | 5 ++- .../gz/rendering/GlobalIlluminationCiVct.hh | 10 ++++-- include/gz/rendering/GlobalIlluminationVct.hh | 5 ++- include/gz/rendering/NativeWindow.hh | 5 ++- include/gz/rendering/base/BaseNativeWindow.hh | 4 +-- .../ogre2/Ogre2GlobalIlluminationCiVct.hh | 2 ++ ogre2/src/CMakeLists.txt | 1 + ogre2/src/Ogre2GlobalIlluminationCiVct.cc | 5 +++ ogre2/src/Ogre2RenderEngine.cc | 2 +- src/GlobalIlluminationBase.cc | 28 +++++++++++++++ src/GlobalIlluminationCiVct.cc | 34 +++++++++++++++++++ src/GlobalIlluminationVct.cc | 28 +++++++++++++++ src/NativeWindow.cc | 28 +++++++++++++++ src/base/BaseNativeWindow.cc | 28 +++++++++++++++ 14 files changed, 177 insertions(+), 8 deletions(-) create mode 100644 src/GlobalIlluminationBase.cc create mode 100644 src/GlobalIlluminationCiVct.cc create mode 100644 src/GlobalIlluminationVct.cc create mode 100644 src/NativeWindow.cc create mode 100644 src/base/BaseNativeWindow.cc diff --git a/include/gz/rendering/GlobalIlluminationBase.hh b/include/gz/rendering/GlobalIlluminationBase.hh index 881512558..87caf5c3c 100644 --- a/include/gz/rendering/GlobalIlluminationBase.hh +++ b/include/gz/rendering/GlobalIlluminationBase.hh @@ -52,8 +52,11 @@ namespace gz STATIC_VISUALS = 1u << 1u }; + /// \brief Constructor + public: GlobalIlluminationBase(); + /// \brief Destructor - public: virtual ~GlobalIlluminationBase() { } + public: virtual ~GlobalIlluminationBase(); /// \brief Initialize the class protected: virtual void Init() = 0; diff --git a/include/gz/rendering/GlobalIlluminationCiVct.hh b/include/gz/rendering/GlobalIlluminationCiVct.hh index a3a2ea401..dbffdf01a 100644 --- a/include/gz/rendering/GlobalIlluminationCiVct.hh +++ b/include/gz/rendering/GlobalIlluminationCiVct.hh @@ -31,8 +31,11 @@ namespace gz class GZ_RENDERING_VISIBLE CiVctCascade { + /// \brief Constructor + public: CiVctCascade(); + /// \brief Destructor - public: virtual ~CiVctCascade() {} + public: virtual ~CiVctCascade(); /// \brief Sets whether to correctly calculate GI occlusion caused /// by occluders against area lights. Consumes more VRAM. @@ -149,8 +152,11 @@ namespace gz DVM_None }; + /// \brief Constructor + public: GlobalIlluminationCiVct(); + /// \brief Destructor - public: virtual ~GlobalIlluminationCiVct() { } + public: virtual ~GlobalIlluminationCiVct(); /// \brief Tells how many times AddCascade will be called. /// You can call it fewer times (i.e. some kb of RAM will be wasted) diff --git a/include/gz/rendering/GlobalIlluminationVct.hh b/include/gz/rendering/GlobalIlluminationVct.hh index be86924a0..a755e5216 100644 --- a/include/gz/rendering/GlobalIlluminationVct.hh +++ b/include/gz/rendering/GlobalIlluminationVct.hh @@ -48,8 +48,11 @@ namespace gz DVM_None }; + /// \brief Constructor + public: GlobalIlluminationVct(); + /// \brief Destructor - public: virtual ~GlobalIlluminationVct() { } + public: virtual ~GlobalIlluminationVct(); /// \brief Resolution of the 3D Voxel. Must be multiple of 2 /// \remarks To avoid wasting RAM, make this function your first call diff --git a/include/gz/rendering/NativeWindow.hh b/include/gz/rendering/NativeWindow.hh index c1b8f6fb1..1e8982fcb 100644 --- a/include/gz/rendering/NativeWindow.hh +++ b/include/gz/rendering/NativeWindow.hh @@ -38,8 +38,11 @@ namespace gz /// on OS and RenderSystem class GZ_RENDERING_VISIBLE NativeWindow { + /// \brief Constructor + public: NativeWindow(); + /// \brief Destructor - public: virtual ~NativeWindow() { } + public: virtual ~NativeWindow(); /// \brief Tells the native window whether it's under focus /// \param[in] _focused True if we acquired focus. False if we lost it diff --git a/include/gz/rendering/base/BaseNativeWindow.hh b/include/gz/rendering/base/BaseNativeWindow.hh index adb664087..55919140c 100644 --- a/include/gz/rendering/base/BaseNativeWindow.hh +++ b/include/gz/rendering/base/BaseNativeWindow.hh @@ -32,9 +32,9 @@ namespace gz class GZ_RENDERING_VISIBLE BaseNativeWindow : public virtual NativeWindow { - protected: BaseNativeWindow() {} + protected: BaseNativeWindow(); - public: virtual ~BaseNativeWindow() {} + public: virtual ~BaseNativeWindow(); // Documentation Inherited. public: virtual void NotifyFocused(bool /*_focused*/) override {} diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2GlobalIlluminationCiVct.hh b/ogre2/include/gz/rendering/ogre2/Ogre2GlobalIlluminationCiVct.hh index dd1b3b3a2..06d05f48c 100644 --- a/ogre2/include/gz/rendering/ogre2/Ogre2GlobalIlluminationCiVct.hh +++ b/ogre2/include/gz/rendering/ogre2/Ogre2GlobalIlluminationCiVct.hh @@ -47,6 +47,8 @@ namespace gz /// \brief Constructor public: explicit Ogre2CiVctCascade(); + public: ~Ogre2CiVctCascade() override; + /// \brief Initializes the cascade /// \param[in] _cascade Cascade we control /// \param[in] _ref Reference to clone settings from (can be nullptr) diff --git a/ogre2/src/CMakeLists.txt b/ogre2/src/CMakeLists.txt index c2cd9f479..466db6983 100644 --- a/ogre2/src/CMakeLists.txt +++ b/ogre2/src/CMakeLists.txt @@ -9,6 +9,7 @@ if (MSVC) # warning is not important since those members do not need to be interfaced # with. set_source_files_properties(${sources} ${gtest_sources} COMPILE_FLAGS "/wd4251") + set_source_files_properties(${sources} ${gtest_sources} COMPILE_FLAGS "/D_SILENCE_STDEXT_HASH_DEPRECATION_WARNINGS") endif() set(engine_name "ogre2") diff --git a/ogre2/src/Ogre2GlobalIlluminationCiVct.cc b/ogre2/src/Ogre2GlobalIlluminationCiVct.cc index eb6555033..2a90fac9c 100644 --- a/ogre2/src/Ogre2GlobalIlluminationCiVct.cc +++ b/ogre2/src/Ogre2GlobalIlluminationCiVct.cc @@ -212,6 +212,11 @@ Ogre2GlobalIlluminationCiVct::~Ogre2GlobalIlluminationCiVct() this->Destroy(); } +////////////////////////////////////////////////// +Ogre2CiVctCascade::~Ogre2CiVctCascade() +{ +} + ////////////////////////////////////////////////// void Ogre2GlobalIlluminationCiVct::Init() { diff --git a/ogre2/src/Ogre2RenderEngine.cc b/ogre2/src/Ogre2RenderEngine.cc index ca68b5af4..8ae00523e 100644 --- a/ogre2/src/Ogre2RenderEngine.cc +++ b/ogre2/src/Ogre2RenderEngine.cc @@ -1343,7 +1343,7 @@ NativeWindowPtr Ogre2RenderEngine::CreateNativeWindow( #ifdef _WIN32 params["externalGLContext"] = std::to_string((size_t)wglGetCurrentContext()); -# elidef __APPLE__ +#elif defined(__APPLE__) params["externalGLContext"] = std::to_string((size_t)CGLGetCurrentContext()); #else diff --git a/src/GlobalIlluminationBase.cc b/src/GlobalIlluminationBase.cc new file mode 100644 index 000000000..f6fb974e6 --- /dev/null +++ b/src/GlobalIlluminationBase.cc @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2023 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 "gz/rendering/GlobalIlluminationBase.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +GlobalIlluminationBase::GlobalIlluminationBase() = default; + +////////////////////////////////////////////////// +GlobalIlluminationBase::~GlobalIlluminationBase() = default; + diff --git a/src/GlobalIlluminationCiVct.cc b/src/GlobalIlluminationCiVct.cc new file mode 100644 index 000000000..293f72c4d --- /dev/null +++ b/src/GlobalIlluminationCiVct.cc @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2023 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 "gz/rendering/GlobalIlluminationCiVct.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +CiVctCascade::CiVctCascade() = default; + +////////////////////////////////////////////////// +CiVctCascade::~CiVctCascade() = default; + +////////////////////////////////////////////////// +GlobalIlluminationCiVct::GlobalIlluminationCiVct() = default; + +////////////////////////////////////////////////// +GlobalIlluminationCiVct::~GlobalIlluminationCiVct() = default; + diff --git a/src/GlobalIlluminationVct.cc b/src/GlobalIlluminationVct.cc new file mode 100644 index 000000000..cf87d4e9d --- /dev/null +++ b/src/GlobalIlluminationVct.cc @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2023 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 "gz/rendering/GlobalIlluminationVct.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +GlobalIlluminationVct::GlobalIlluminationVct() = default; + +////////////////////////////////////////////////// +GlobalIlluminationVct::~GlobalIlluminationVct() = default; + diff --git a/src/NativeWindow.cc b/src/NativeWindow.cc new file mode 100644 index 000000000..0a7f97d66 --- /dev/null +++ b/src/NativeWindow.cc @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2023 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 "gz/rendering/NativeWindow.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +NativeWindow::NativeWindow() = default; + +////////////////////////////////////////////////// +NativeWindow::~NativeWindow() = default; + diff --git a/src/base/BaseNativeWindow.cc b/src/base/BaseNativeWindow.cc new file mode 100644 index 000000000..76bb7d26d --- /dev/null +++ b/src/base/BaseNativeWindow.cc @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2023 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 "gz/rendering/base/BaseNativeWindow.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +BaseNativeWindow::BaseNativeWindow() = default; + +////////////////////////////////////////////////// +BaseNativeWindow::~BaseNativeWindow() = default; + From c108afb352e465164d1799e2077a34d614f0cac7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Sep 2023 18:30:28 -0700 Subject: [PATCH 4/8] Update README (#907) --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index a02a7cce8..65d93b082 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ Build | Status -- | -- Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-rendering/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-rendering/branch/default) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_rendering-ci-main-focal-amd64)](https://build.osrfoundation.org/job/ignition_rendering-ci-main-focal-amd64) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_rendering-ci-main-jammy-amd64)](https://build.osrfoundation.org/job/gz_rendering-ci-main-jammy-amd64) Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_rendering-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_rendering-ci-main-homebrew-amd64) Windows | [![Build Status](https://build.osrfoundation.org/job/ign_rendering-ci-win/badge/icon)](https://build.osrfoundation.org/job/ign_rendering-ci-win/) @@ -47,12 +47,12 @@ of libraries designed to rapidly develop robot applications. # Install -See the [installation tutorial](https://gazebosim.org/api/rendering/5.0/installation.html). +See the [installation tutorial](https://gazebosim.org/api/rendering/8/installation.html). # Usage The Gazebo Rendering API can be found in the documentation. See the -[installation tutorial](https://gazebosim.org/api/rendering/5.0/installation.html) +[installation tutorial](https://gazebosim.org/api/rendering/8/installation.html) on how to build the documentation files using Doxygen. You can also take a look at the sample applications in the `examples` folder. @@ -74,7 +74,7 @@ Rendering engine plugin implementation code is stored in their own folders * `ogre` : OGRE 1.x rendering engine plugin -* `ogre2` : OGRE 2.x rendering engine plugin (available in versions >= gz-rendering1) +* `ogre2` : OGRE-Next 2.x rendering engine plugin * `optix` : OptiX rendering engine plugin From 8c1fd27747a2c12eca0b111bf961047a5fa5b1a6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 15 Sep 2023 07:59:44 -0700 Subject: [PATCH 5/8] ogre2: use CMAKE_INSTALL_RPATH (#909) Sets the ogre2 target INSTALL_RPATH to a copy of CMAKE_INSTALL_RPATH with OGRE_LIBRARY_DIRS appended, instead of just the latter path. This helps with packaging on macOS. Signed-off-by: Steve Peters --- ogre2/src/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ogre2/src/CMakeLists.txt b/ogre2/src/CMakeLists.txt index 466db6983..ef328190a 100644 --- a/ogre2/src/CMakeLists.txt +++ b/ogre2/src/CMakeLists.txt @@ -28,10 +28,14 @@ set_property( target_compile_definitions(${ogre2_target} PRIVATE OGRE_IGNORE_UNKNOWN_DEBUG) +# Add OGRE2_LIBRARY_DIRS to INSTALL_RPATH +# Append to a copy of CMAKE_INSTALL_RPATH since that is the default +set(ogre2_target_install_rpath ${CMAKE_INSTALL_RPATH}) +list(APPEND ogre2_target_install_rpath ${OGRE2_LIBRARY_DIRS}) set_property( TARGET ${ogre2_target} PROPERTY INSTALL_RPATH - ${OGRE2_LIBRARY_DIRS} + ${ogre2_target_install_rpath} ) target_include_directories(${ogre2_target} From 9d524c2191fd5f09f45e4972b69365c77faceeec Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 18 Sep 2023 09:46:42 -0500 Subject: [PATCH 6/8] Include the CGL header on macOS (#911) Signed-off-by: Michael Carroll --- ogre2/src/Ogre2RenderEngine.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ogre2/src/Ogre2RenderEngine.cc b/ogre2/src/Ogre2RenderEngine.cc index 8ae00523e..c83edd987 100644 --- a/ogre2/src/Ogre2RenderEngine.cc +++ b/ogre2/src/Ogre2RenderEngine.cc @@ -68,6 +68,10 @@ #include #endif +#if defined(__APPLE__) +#include +#endif + class GZ_RENDERING_OGRE2_HIDDEN gz::rendering::Ogre2RenderEnginePrivate { From 565556680d119ac8bc57e74bc847b57e1e71fecc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 18 Sep 2023 16:05:55 -0700 Subject: [PATCH 7/8] Fix projector FOV, orientation, and crashes (#913) * Fix FOV calculation, projected texture orientation, crash when heighmaps are present Signed-off-by: Ian Chen Co-authored-by: Addisu Z. Taddese --- ogre2/src/Ogre2Projector.cc | 9 +- .../media/Hlms/Terra/GLSL/PixelShader_ps.glsl | 23 +++- .../Terra/gz/500.gz_Structs_piece_all.any | 3 + test/integration/projector.cc | 107 ++++++++++++++++-- 4 files changed, 131 insertions(+), 11 deletions(-) diff --git a/ogre2/src/Ogre2Projector.cc b/ogre2/src/Ogre2Projector.cc index 1e25281e8..6052acc5d 100644 --- a/ogre2/src/Ogre2Projector.cc +++ b/ogre2/src/Ogre2Projector.cc @@ -110,15 +110,16 @@ Ogre2Projector::Ogre2Projector() ///////////////////////////////////////////////// Ogre2Projector::~Ogre2Projector() { - this->SetEnabled(false); - if (!this->scene->IsInitialized()) return; + this->SetEnabled(false); + for (const auto &ogreCamIt : this->dataPtr->camerasWithListener) { Ogre::IdString camName = ogreCamIt.second; auto ogreCam = this->scene->OgreSceneManager()->findCameraNoThrow(camName); + if (ogreCam) ogreCam->removeListener(this->dataPtr->listener.get()); } this->dataPtr->camerasWithListener.clear(); @@ -230,6 +231,7 @@ void Ogre2Projector::CreateProjector() { this->dataPtr->decalNode = this->ogreNode->createChildSceneNode(); this->dataPtr->decalNode->roll(Ogre::Degree(90)); + this->dataPtr->decalNode->yaw(Ogre::Degree(180)); this->dataPtr->decal = this->scene->OgreSceneManager()->createDecal(); this->dataPtr->decalNode->attachObject(this->dataPtr->decal); @@ -275,7 +277,8 @@ void Ogre2Projector::CreateProjector() // approximate frustum size common::Image image(this->textureName); - const double aspectRatio = image.Width() / image.Height(); + const double aspectRatio = static_cast(image.Width()) / + static_cast(image.Height()); const double vfov = 2.0 * atan(tan(this->hfov.Radian() / 2.0) / aspectRatio); diff --git a/ogre2/src/media/Hlms/Terra/GLSL/PixelShader_ps.glsl b/ogre2/src/media/Hlms/Terra/GLSL/PixelShader_ps.glsl index 3b34287df..186ec618f 100644 --- a/ogre2/src/media/Hlms/Terra/GLSL/PixelShader_ps.glsl +++ b/ogre2/src/media/Hlms/Terra/GLSL/PixelShader_ps.glsl @@ -108,7 +108,28 @@ vulkan( layout( ogre_s@value(terrainNormals) ) uniform sampler samplerStateTerra @insertpiece( DeclParallaxLocalCorrect ) @end -@insertpiece( DeclDecalsSamplers ) +// The DeclDecalsSamplers insertpiece does not seem to do anything. +// The contents from DeclDecalsSamplers are manually pasted below +// This prevents a crash when there are decals in the scene +// @insertpiece( DeclDecalsSamplers ) +@property( hlms_forwardplus ) +@property( hlms_enable_decals ) + @piece( DeclDecalsSamplers ) + @property( syntax == glslvk ) + layout( ogre_s@value(decalsSampler) ) uniform sampler decalsSampler; + @end + @property( hlms_decals_diffuse ) vulkan_layout( ogre_t@value(decalsDiffuseTex) ) uniform texture2DArray decalsDiffuseTex;@end + @property( hlms_decals_normals )vulkan_layout( ogre_t@value(decalsNormalsTex) ) uniform texture2DArray decalsNormalsTex;@end + @property( hlms_decals_diffuse == hlms_decals_emissive ) + #define decalsEmissiveTex decalsDiffuseTex + @end + @property( hlms_decals_emissive && hlms_decals_diffuse != hlms_decals_emissive ) + vulkan_layout( ogre_t@value(decalsEmissiveTex) ) uniform texture2DArray decalsEmissiveTex; + @end + @end +@end +@end + @insertpiece( DeclShadowMapMacros ) @insertpiece( DeclShadowSamplers ) diff --git a/ogre2/src/media/Hlms/Terra/gz/500.gz_Structs_piece_all.any b/ogre2/src/media/Hlms/Terra/gz/500.gz_Structs_piece_all.any index 60ca19c92..659d3bce2 100644 --- a/ogre2/src/media/Hlms/Terra/gz/500.gz_Structs_piece_all.any +++ b/ogre2/src/media/Hlms/Terra/gz/500.gz_Structs_piece_all.any @@ -8,4 +8,7 @@ @piece( custom_VStoPS_terra ) INTERPOLANT( float localHeight, @counter(texcoord) ); + + // added to prevent crash when decals are in the scene + INTERPOLANT( float3 normal, @counter(texcoord) ); @end diff --git a/test/integration/projector.cc b/test/integration/projector.cc index 134380c4f..4fc2d0676 100644 --- a/test/integration/projector.cc +++ b/test/integration/projector.cc @@ -20,8 +20,10 @@ #include "CommonRenderingTest.hh" #include +#include #include "gz/rendering/Camera.hh" +#include "gz/rendering/Heightmap.hh" #include "gz/rendering/Material.hh" #include "gz/rendering/Projector.hh" #include "gz/rendering/Scene.hh" @@ -47,11 +49,6 @@ TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Visibility)) ScenePtr scene = engine->CreateScene("scene"); ASSERT_NE(nullptr, scene); - // Projector and can only be accessed by the scene extension API - // in gz-rendering7 - if (!scene->Extension()) - return; - scene->SetBackgroundColor(0, 0, 0); scene->SetAmbientLight(1, 1, 1); @@ -152,11 +149,9 @@ TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Visibility)) common::Image imgA; imgA.SetFromData(dataA, width, height, common::Image::RGB_INT8); - imgA.SavePNG("imageA.png"); common::Image imgB; imgB.SetFromData(dataB, width, height, common::Image::RGB_INT8); - imgB.SavePNG("imageB.png"); for (unsigned int i = 0; i < height; ++i) { @@ -184,3 +179,101 @@ TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Visibility)) // Clean up engine->DestroyScene(scene); } + +///////////////////////////////////////////////// +TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Heightmap)) +{ + // This test checks that projectors and heightmaps can co-exist + ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + + scene->SetBackgroundColor(0, 0, 0); + scene->SetAmbientLight(1, 1, 1); + + VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + DirectionalLightPtr light0 = scene->CreateDirectionalLight(); + light0->SetDirection(0.0, 0.0, -1); + light0->SetDiffuseColor(1.0, 1.0, 1.0); + light0->SetSpecularColor(1.0, 1.0, 1.0); + root->AddChild(light0); + + CameraPtr cameraA = scene->CreateCamera(); + ASSERT_NE(nullptr, cameraA); + cameraA->SetWorldPosition(0, 0, -2); + cameraA->SetWorldRotation(0, GZ_PI / 2.0, 0); + cameraA->SetVisibilityMask(0x01); + cameraA->SetImageWidth(256); + cameraA->SetImageHeight(256); + root->AddChild(cameraA); + + // create projector + std::string textureRed = common::joinPaths( + TEST_MEDIA_PATH, "materials", "textures", + "red_texture.png"); + ProjectorPtr projectorA = scene->CreateProjector(); + ASSERT_NE(nullptr, projectorA); + projectorA->SetNearClipPlane(1.0); + projectorA->SetFarClipPlane(6.0); + projectorA->SetTexture(textureRed); + projectorA->SetVisibilityFlags(0x01); + projectorA->SetWorldRotation(0, GZ_PI / 2.0, 0); + root->AddChild(projectorA); + + // create ImageHeightmap + auto data = std::make_shared(); + data->Load(common::joinPaths(TEST_MEDIA_PATH, "heightmap_bowl.png")); + + HeightmapDescriptor desc; + desc.SetName("example_bowl"); + desc.SetData(data); + desc.SetSize({ 17, 17, 7.0 }); + desc.SetSampling(2u); + desc.SetUseTerrainPaging(false); + + const auto textureImage = + common::joinPaths(TEST_MEDIA_PATH, "materials", "textures", "texture.png"); + const auto normalImage = common::joinPaths(TEST_MEDIA_PATH, "materials", + "textures", "flat_normal.png"); + + HeightmapTexture textureA; + textureA.SetSize(1.0); + textureA.SetDiffuse(textureImage); + textureA.SetNormal(normalImage); + desc.AddTexture(textureA); + + HeightmapBlend blendA; + blendA.SetMinHeight(2.0); + blendA.SetFadeDistance(5.0); + desc.AddBlend(blendA); + + HeightmapTexture textureB; + textureB.SetSize(1.0); + textureB.SetDiffuse(textureImage); + textureB.SetNormal(normalImage); + desc.AddTexture(textureB); + + HeightmapBlend blendB; + blendB.SetMinHeight(4.0); + blendB.SetFadeDistance(5.0); + desc.AddBlend(blendB); + + HeightmapTexture textureC; + textureC.SetSize(1.0); + textureC.SetDiffuse(textureImage); + textureC.SetNormal(normalImage); + desc.AddTexture(textureC); + auto heightmapGeom = scene->CreateHeightmap(desc); + auto vis = scene->CreateVisual(); + vis->AddGeometry(heightmapGeom); + root->AddChild(vis); + + // render once to update scene graph and make sure + // there are no crashes + Image imageA = cameraA->CreateImage(); + EXPECT_NO_THROW(cameraA->Capture(imageA)); + + // Clean up + engine->DestroyScene(scene); +} From 4190ff68ef51dbe8baa0e360402afb12c2736d0a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Sep 2023 13:18:09 -0700 Subject: [PATCH 8/8] Remove gazebo scene viewer example (#915) * remove gazebo scene viewer example --------- Signed-off-by: Ian Chen --- examples/gazebo_scene_viewer/CMakeLists.txt | 53 - examples/gazebo_scene_viewer/CameraWindow.cc | 213 -- examples/gazebo_scene_viewer/CameraWindow.hh | 41 - examples/gazebo_scene_viewer/GazeboDemo.cc | 116 - .../gazebo_scene_viewer/GazeboWorldDemo.cc | 116 - examples/gazebo_scene_viewer/SceneManager.cc | 2136 ----------------- examples/gazebo_scene_viewer/SceneManager.hh | 140 -- .../SceneManagerPrivate.hh | 473 ---- .../gazebo_scene_viewer/falling_objects.world | 398 --- tutorials.md.in | 1 - tutorials/11_gazebo_scene_viewer_tutorial.md | 72 - 11 files changed, 3759 deletions(-) delete mode 100644 examples/gazebo_scene_viewer/CMakeLists.txt delete mode 100644 examples/gazebo_scene_viewer/CameraWindow.cc delete mode 100644 examples/gazebo_scene_viewer/CameraWindow.hh delete mode 100644 examples/gazebo_scene_viewer/GazeboDemo.cc delete mode 100644 examples/gazebo_scene_viewer/GazeboWorldDemo.cc delete mode 100644 examples/gazebo_scene_viewer/SceneManager.cc delete mode 100644 examples/gazebo_scene_viewer/SceneManager.hh delete mode 100644 examples/gazebo_scene_viewer/SceneManagerPrivate.hh delete mode 100644 examples/gazebo_scene_viewer/falling_objects.world delete mode 100644 tutorials/11_gazebo_scene_viewer_tutorial.md diff --git a/examples/gazebo_scene_viewer/CMakeLists.txt b/examples/gazebo_scene_viewer/CMakeLists.txt deleted file mode 100644 index 2f851c29c..000000000 --- a/examples/gazebo_scene_viewer/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -project(gz-rendering-gazebo-scene-viewer) -find_package(gz-rendering8 REQUIRED) -find_package(gazebo REQUIRED) - -include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS}) - -find_package(GLUT REQUIRED) -include_directories(SYSTEM ${GLUT_INCLUDE_DIRS}) -link_directories(${GLUT_LIBRARY_DIRS}) - -find_package(OpenGL REQUIRED) -include_directories(SYSTEM ${OpenGL_INCLUDE_DIRS}) -link_directories(${OpenGL_LIBRARY_DIRS}) - -if (NOT APPLE) - find_package(GLEW REQUIRED) - include_directories(SYSTEM ${GLEW_INCLUDE_DIRS}) - link_directories(${GLEW_LIBRARY_DIRS}) - set(STD_CXX_FS_LIBRARIES "stdc++fs") -endif() - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations") - -add_executable(gazebo_scene_viewer - CameraWindow.cc - GazeboDemo.cc - SceneManager.cc -) - -target_link_libraries(gazebo_scene_viewer - ${GLUT_LIBRARIES} - ${OPENGL_LIBRARIES} - ${GLEW_LIBRARIES} - ${GAZEBO_LIBRARIES} - ${GZ-RENDERING_LIBRARIES} - ${STD_CXX_FS_LIBRARIES} -) - -add_executable(gazebo_scene_viewer2_demo - CameraWindow.cc - GazeboWorldDemo.cc - SceneManager.cc -) - -target_link_libraries(gazebo_scene_viewer2_demo - ${GLUT_LIBRARIES} - ${OPENGL_LIBRARIES} - ${GLEW_LIBRARIES} - ${GAZEBO_LIBRARIES} - ${GZ-RENDERING_LIBRARIES} - ${STD_CXX_FS_LIBRARIES} -) diff --git a/examples/gazebo_scene_viewer/CameraWindow.cc b/examples/gazebo_scene_viewer/CameraWindow.cc deleted file mode 100644 index 96e37f437..000000000 --- a/examples/gazebo_scene_viewer/CameraWindow.cc +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Copyright (C) 2015 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. - * - */ -#if __APPLE__ - #include - #include - #include -#else - #include - #include - #include -#endif - -#if !defined(__APPLE__) && !defined(_WIN32) - #include - #undef Status -#endif - -#include -#include - -#include -#include -#include - -#include "CameraWindow.hh" -#include "SceneManager.hh" - -#define KEY_ESC 27 -#define KEY_TAB 9 - -////////////////////////////////////////////////// -unsigned int imgw = 0; -unsigned int imgh = 0; - -std::vector g_cameras; -gz::rendering::CameraPtr g_camera; -gz::rendering::CameraPtr g_currCamera; -unsigned int g_cameraIndex = 0; -gz::ImagePtr g_image; - -bool g_initContext = false; - -#if __APPLE__ - CGLContextObj g_context; - CGLContextObj g_glutContext; -#elif _WIN32 -#else - GLXContext g_context; - Display *g_display; - GLXDrawable g_drawable; - GLXContext g_glutContext; - Display *g_glutDisplay; - GLXDrawable g_glutDrawable; -#endif - -double g_offset = 0.0; - -////////////////////////////////////////////////// -void GlutRun(std::vector _cameras) -{ -#if __APPLE__ - g_context = CGLGetCurrentContext(); -#elif _WIN32 -#else - g_context = glXGetCurrentContext(); - g_display = glXGetCurrentDisplay(); - g_drawable = glXGetCurrentDrawable(); -#endif - - g_cameras = _cameras; - GlutInitCamera(_cameras[0]); - GlutInitContext(); - GlutPrintUsage(); - -#if __APPLE__ - g_glutContext = CGLGetCurrentContext(); -#elif _WIN32 -#else - g_glutDisplay = glXGetCurrentDisplay(); - g_glutDrawable = glXGetCurrentDrawable(); - g_glutContext = glXGetCurrentContext(); -#endif - - glutMainLoop(); -} - -////////////////////////////////////////////////// -void GlutDisplay() -{ -#if __APPLE__ - CGLSetCurrentContext(g_context); -#elif _WIN32 -#else - if (g_display) - { - glXMakeCurrent(g_display, g_drawable, g_context); - } -#endif - - g_cameras[g_cameraIndex]->Capture(*g_image); - -#if __APPLE__ - CGLSetCurrentContext(g_glutContext); -#elif _WIN32 -#else - glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext); -#endif - - unsigned char *data = g_image->Data(); - - glClearColor(0.5, 0.5, 0.5, 1); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glPixelZoom(1, -1); - glRasterPos2f(-1, 1); - glDrawPixels(imgw, imgh, GL_RGB, GL_UNSIGNED_BYTE, data); - - glutSwapBuffers(); -} - -////////////////////////////////////////////////// -void GlutIdle() -{ -#if __APPLE__ - CGLSetCurrentContext(g_context); -#elif _WIN32 -#else - if (g_display) - { - glXMakeCurrent(g_display, g_drawable, g_context); - } -#endif - - gz::rendering::SceneManager* manager = - gz::rendering::SceneManager::Instance(); - - manager->UpdateScenes(); - -#if __APPLE__ - CGLSetCurrentContext(g_glutContext); -#elif _WIN32 -#else - glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext); -#endif - - glutPostRedisplay(); -} - -////////////////////////////////////////////////// -void GlutKeyboard(unsigned char _key, int, int) -{ - if (_key == KEY_ESC || _key == 'q' || _key == 'Q') - { - // stop transport - gazebo::transport::stop(); - gazebo::transport::fini(); - exit(0); - } - else if (_key == KEY_TAB) - { - g_cameraIndex = (g_cameraIndex + 1) % g_cameras.size(); - } -} - -////////////////////////////////////////////////// -void GlutReshape(int, int) -{ -} - -////////////////////////////////////////////////// -void GlutInitCamera(gz::rendering::CameraPtr _camera) -{ - g_camera = _camera; - imgw = g_camera->ImageWidth(); - imgh = g_camera->ImageHeight(); - gz::rendering::Image image = g_camera->CreateImage(); - g_image = std::make_shared(image); - g_camera->Capture(*g_image); -} - -////////////////////////////////////////////////// -void GlutInitContext() -{ - glutInitDisplayMode(GLUT_DOUBLE); - glutInitWindowPosition(0, 0); - glutInitWindowSize(imgw, imgh); - glutCreateWindow("Gazebo"); - glutDisplayFunc(GlutDisplay); - glutIdleFunc(GlutIdle); - glutKeyboardFunc(GlutKeyboard); - glutReshapeFunc(GlutReshape); -} - -void GlutPrintUsage() -{ - std::cout << "===============================" << std::endl; - std::cout << " TAB - Switch render engines " << std::endl; - std::cout << " ESC - Exit " << std::endl; - std::cout << "===============================" << std::endl; -} diff --git a/examples/gazebo_scene_viewer/CameraWindow.hh b/examples/gazebo_scene_viewer/CameraWindow.hh deleted file mode 100644 index 0ed0bb3e4..000000000 --- a/examples/gazebo_scene_viewer/CameraWindow.hh +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2015 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 GZ_RENDERING_EXAMPLES_CAMERAWINDOW_HH_ -#define GZ_RENDERING_EXAMPLES_CAMERAWINDOW_HH_ - -#include -#include "gz/rendering/RenderTypes.hh" - -namespace gz = gz::rendering; - -void GlutRun(std::vector _cameras); - -void GlutDisplay(); - -void GlutIdle(); - -void GlutKeyboard(unsigned char _key, int _x, int _y); - -void GlutReshape(int _width, int _height); - -void GlutInitCamera(gz::CameraPtr _camera); - -void GlutInitContext(); - -void GlutPrintUsage(); - -#endif diff --git a/examples/gazebo_scene_viewer/GazeboDemo.cc b/examples/gazebo_scene_viewer/GazeboDemo.cc deleted file mode 100644 index b85718c9d..000000000 --- a/examples/gazebo_scene_viewer/GazeboDemo.cc +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (C) 2015 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. - * - */ - -#if defined(__APPLE__) - #include - #include -#elif not defined(_WIN32) - #include - #include - #include -#endif - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "CameraWindow.hh" -#include "SceneManager.hh" - -using namespace gz; -using namespace rendering; - -void Connect() -{ - gazebo::common::Console::SetQuiet(false); - gazebo::transport::init(); - gazebo::transport::run(); - - SceneManager *manager = SceneManager::Instance(); - manager->Load(); - manager->Init(); -} - -ScenePtr CreateScene(const std::string &_engine) -{ - RenderEngine *engine = rendering::engine(_engine); - if (!engine) - { - std::cout << "Engine '" << _engine - << "' is not supported" << std::endl; - return ScenePtr(); - } - - ScenePtr scene = engine->CreateScene("scene"); - SceneManager::Instance()->AddScene(scene); - return scene; -} - -CameraPtr CreateCamera(const std::string &_engine) -{ - ScenePtr scene = CreateScene(_engine); - if (!scene) - return CameraPtr(); - VisualPtr root = scene->RootVisual(); - - CameraPtr camera = scene->CreateCamera("camera"); - camera->SetLocalPosition(5.0, -5.0, 2.0); - camera->SetLocalRotation(0.0, 0.27, 2.36); - camera->SetImageWidth(640); - camera->SetImageHeight(480); - camera->SetAntiAliasing(2); - camera->SetAspectRatio(1.333); - camera->SetHFOV(GZ_PI / 2); - root->AddChild(camera); - - return camera; -} - -int main(int _argc, char** _argv) -{ - glutInit(&_argc, _argv); - - // Expose engine name to command line because we can't instantiate both - // ogre and ogre2 at the same time - std::string ogreEngineName("ogre"); - if (_argc > 1) - { - ogreEngineName = _argv[1]; - } - - Connect(); - std::vector cameras; - std::vector engineNames; - - engineNames.push_back(ogreEngineName); - - for (auto engineName : engineNames) - { - CameraPtr camera = CreateCamera(engineName); - if (camera) - cameras.push_back(camera); - } - - GlutRun(cameras); - return 0; -} diff --git a/examples/gazebo_scene_viewer/GazeboWorldDemo.cc b/examples/gazebo_scene_viewer/GazeboWorldDemo.cc deleted file mode 100644 index 79866de69..000000000 --- a/examples/gazebo_scene_viewer/GazeboWorldDemo.cc +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (C) 2015 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. - * - */ - -#if defined(__APPLE__) - #include - #include -#elif not defined(_WIN32) - #include - #include - #include -#endif - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "CameraWindow.hh" -#include "SceneManager.hh" - -using namespace gz; -using namespace rendering; - -void Connect() -{ - common::Console::SetVerbosity(4); - gazebo::transport::init(); - gazebo::transport::run(); - - SceneManager* manager = SceneManager::Instance(); - manager->Load(); - manager->Init(); -} - -ScenePtr CreateScene(const std::string &_engine) -{ - RenderEngine *engine = rendering::engine(_engine); - if (!engine) - { - std::cout << "Engine '" << _engine - << "' is not supported" << std::endl; - return ScenePtr(); - } - - ScenePtr scene = engine->CreateScene("scene"); - SceneManager::Instance()->AddScene(scene); - return scene; -} - -CameraPtr CreateCamera(const std::string &_engine) -{ - ScenePtr scene = CreateScene(_engine); - if (!scene) - return CameraPtr(); - VisualPtr root = scene->RootVisual(); - - CameraPtr camera = scene->CreateCamera("camera"); - camera->SetLocalPosition(-1.0, 1.0, 0.0); - camera->SetLocalRotation(0.0, 0.35, -0.175); - camera->SetImageWidth(640); - camera->SetImageHeight(480); - camera->SetAntiAliasing(2); - camera->SetAspectRatio(1.333); - camera->SetHFOV(GZ_PI / 3); - root->AddChild(camera); - - return camera; -} - -int main(int _argc, char** _argv) -{ - glutInit(&_argc, _argv); - - // Expose engine name to command line because we can't instantiate both - // ogre and ogre2 at the same time - std::string ogreEngineName("ogre"); - if (_argc > 1) - { - ogreEngineName = _argv[1]; - } - - Connect(); - std::vector cameras; - std::vector engineNames; - - engineNames.push_back(ogreEngineName); - - for (auto engineName : engineNames) - { - CameraPtr camera = CreateCamera(engineName); - if (camera) - cameras.push_back(camera); - } - - GlutRun(cameras); - return 0; -} diff --git a/examples/gazebo_scene_viewer/SceneManager.cc b/examples/gazebo_scene_viewer/SceneManager.cc deleted file mode 100644 index 09a486317..000000000 --- a/examples/gazebo_scene_viewer/SceneManager.cc +++ /dev/null @@ -1,2136 +0,0 @@ -/* - * Copyright (C) 2015 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 "SceneManager.hh" -#include "SceneManagerPrivate.hh" - -using namespace gz; -using namespace rendering; - -////////////////////////////////////////////////// -SceneManager::SceneManager() : - pimpl(new SceneManagerPrivate) -{ -} - -////////////////////////////////////////////////// -SceneManager::~SceneManager() -{ - this->Fini(); - delete this->pimpl; -} - -////////////////////////////////////////////////// -void SceneManager::Load() -{ - this->pimpl->Load(); -} - -////////////////////////////////////////////////// -void SceneManager::Init() -{ - this->pimpl->Init(); -} - -////////////////////////////////////////////////// -void SceneManager::Fini() -{ - this->pimpl->Fini(); -} - -////////////////////////////////////////////////// -unsigned int SceneManager::SceneCount() const -{ - return this->pimpl->SceneCount(); -} - -////////////////////////////////////////////////// -bool SceneManager::HasScene(unsigned int _id) const -{ - return this->pimpl->HasScene(_id); -} - -////////////////////////////////////////////////// -bool SceneManager::HasScene(const std::string &_name) const -{ - return this->pimpl->HasScene(_name); -} - -////////////////////////////////////////////////// -bool SceneManager::HasScene(ConstScenePtr _scene) const -{ - return this->pimpl->HasScene(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::Scene(unsigned int _id) const -{ - return this->pimpl->Scene(_id); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::Scene(const std::string &_name) const -{ - return this->pimpl->Scene(_name); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::SceneAt(unsigned int _index) const -{ - return this->pimpl->SceneAt(_index); -} - -////////////////////////////////////////////////// -void SceneManager::AddScene(ScenePtr _scene) -{ - this->pimpl->AddScene(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::RemoveScene(unsigned int _id) -{ - return this->pimpl->RemoveScene(_id); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::RemoveScene(const std::string &_name) -{ - return this->pimpl->RemoveScene(_name); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::RemoveScene(ScenePtr _scene) -{ - return this->pimpl->RemoveScene(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SceneManager::RemoveSceneAt(unsigned int _index) -{ - return this->pimpl->RemoveSceneAt(_index); -} - -////////////////////////////////////////////////// -void SceneManager::RemoveScenes() -{ - this->pimpl->RemoveScenes(); -} - -////////////////////////////////////////////////// -void SceneManager::UpdateScenes() -{ - this->pimpl->UpdateScenes(); -} - -////////////////////////////////////////////////// -SceneManagerPrivate::SceneManagerPrivate() : - currentSceneManager(new CurrentSceneManager), - newSceneManager(new NewSceneManager), - sceneRequestId(-1), - promotionNeeded(false) -{ -} - -////////////////////////////////////////////////// -SceneManagerPrivate::~SceneManagerPrivate() -{ - newSceneManager.reset(); - currentSceneManager.reset(); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::Load() -{ -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::Init() -{ -//! [init scene manager] - // listen for pre-render events - this->preRenderConn = gazebo::event::Events::ConnectPreRender( - std::bind(&SceneManagerPrivate::UpdateScenes, this)); - - // setup transport communication node - this->transportNode = gazebo::transport::NodePtr( - new gazebo::transport::Node()); - this->transportNode->Init(); - - // create publisher for sending scene request - this->requestPub = - this->transportNode->Advertise("~/request"); - - // listen for deletion requests - this->requestSub = this->transportNode->Subscribe("~/request", - &SceneManagerPrivate::OnRequest, this); - - // listen for scene & deletion requests responses - this->responseSub = this->transportNode->Subscribe("~/response", - &SceneManagerPrivate::OnResponse, this); - - // listen for to light updates - this->lightSub = this->transportNode->Subscribe("~/light", - &SceneManagerPrivate::OnLightUpdate, this); - - // TODO(anyone): handle non-local model info - - // listen for to model updates - this->modelSub = this->transportNode->Subscribe("~/model/info", - &SceneManagerPrivate::OnModelUpdate, this); - - // listen for to joint updates - this->jointSub = this->transportNode->Subscribe("~/joint", - &SceneManagerPrivate::OnJointUpdate, this); - - // listen for to visual updates - this->visualSub = this->transportNode->Subscribe("~/visual", - &SceneManagerPrivate::OnVisualUpdate, this); - - // listen for to sensor updates - this->sensorSub = this->transportNode->Subscribe("~/sensor", - &SceneManagerPrivate::OnSensorUpdate, this); - - // TODO(anyone): handle non-local pose info - - // listen for to pose updates - this->poseSub = this->transportNode->Subscribe("~/pose/local/info", - &SceneManagerPrivate::OnPoseUpdate, this); -//! [init scene manager] -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::Fini() -{ - // block all message receival during cleanup - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - this->transportNode->Fini(); -} - -////////////////////////////////////////////////// -unsigned int SceneManagerPrivate::SceneCount() const -{ - unsigned int count = 0; - count += this->currentSceneManager->SceneCount(); - count += this->newSceneManager->SceneCount(); - return count; -} - -////////////////////////////////////////////////// -bool SceneManagerPrivate::HasScene(unsigned int _id) const -{ - return this->currentSceneManager->HasScene(_id) || - this->newSceneManager->HasScene(_id); -} - -////////////////////////////////////////////////// -bool SceneManagerPrivate::HasScene(const std::string &_name) const -{ - return this->currentSceneManager->HasScene(_name) || - this->newSceneManager->HasScene(_name); -} - -////////////////////////////////////////////////// -bool SceneManagerPrivate::HasScene(ConstScenePtr _scene) const -{ - return this->currentSceneManager->HasScene(_scene) || - this->newSceneManager->HasScene(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::Scene(unsigned int _id) const -{ - ScenePtr scene = this->currentSceneManager->Scene(_id); - return (scene) ? scene : this->newSceneManager->Scene(_id); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::Scene(const std::string &_name) const -{ - ScenePtr scene = this->currentSceneManager->Scene(_name); - return (scene) ? scene : this->newSceneManager->Scene(_name); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::SceneAt(unsigned int _index) const -{ - ScenePtr scene = this->currentSceneManager->SceneAt(_index); - return (scene) ? scene : this->newSceneManager->SceneAt(_index); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::AddScene(ScenePtr _scene) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - this->newSceneManager->AddScene(_scene); - - // check if new request needed - if (this->sceneRequestId < 0) - { - this->SendSceneRequest(); - } -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::RemoveScene(unsigned int _id) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - ScenePtr scene = this->currentSceneManager->RemoveScene(_id); - return (scene) ? scene : this->newSceneManager->RemoveScene(_id); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::RemoveScene(const std::string &_name) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - ScenePtr scene = this->currentSceneManager->RemoveScene(_name); - return (scene) ? scene : this->newSceneManager->RemoveScene(_name); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::RemoveScene(ScenePtr _scene) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - ScenePtr scene = this->currentSceneManager->RemoveScene(_scene); - return (scene) ? scene : this->newSceneManager->RemoveScene(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SceneManagerPrivate::RemoveSceneAt(unsigned int _index) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - ScenePtr scene = this->currentSceneManager->RemoveSceneAt(_index); - return (scene) ? scene : this->newSceneManager->RemoveSceneAt(_index); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::RemoveScenes() -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - this->currentSceneManager->RemoveScenes(); - this->newSceneManager->RemoveScenes(); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::UpdateScenes() -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - this->currentSceneManager->UpdateScenes(); - - // check if scene reponse received - if (this->promotionNeeded) - { - this->newSceneManager->UpdateScenes(); - this->PromoteNewScenes(); - this->promotionNeeded = false; - } -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::SendSceneRequest() -{ - gazebo::msgs::Request *request = gazebo::msgs::CreateRequest("scene_info"); - this->sceneRequestId = request->id(); - this->requestPub->Publish(*request); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnRequest(::ConstRequestPtr &_requestMsg) -{ - // check if deletion request - if (_requestMsg->request() == "entity_delete") - { - // record details & wait for response - unsigned int requestId = _requestMsg->id(); - std::string name = _requestMsg->data(); - this->requestedRemovals[requestId] = name; - } -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnResponse(::ConstResponsePtr &_responseMsg) -{ - // check if response to our scene request - if (_responseMsg->id() == this->sceneRequestId) - { - this->OnSceneResponse(_responseMsg); - } - - // else check if response to a delete request - else if (_responseMsg->request() == "entity_delete") - { - this->OnRemovalResponse(_responseMsg); - } -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnSceneResponse(::ConstResponsePtr &_responseMsg) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - // pass scene response to new scene manager - const std::string &data = _responseMsg->serialized_data(); - this->newSceneManager->SetSceneData(data); - - // update state - this->promotionNeeded = true; - this->sceneRequestId = -1; -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnRemovalResponse(::ConstResponsePtr &_responseMsg) -{ - // TODO(anyone): check if message sent after scene response - - unsigned int requestId = _responseMsg->id(); - - // check if delete was successful - if (_responseMsg->response() == "success") - { - std::string name = this->requestedRemovals[requestId]; - this->OnRemovalUpdate(name); - } - - // delete request regardless - this->requestedRemovals.erase(requestId); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnSceneUpdate(::ConstScenePtr) -{ - // block all message receival during update - std::lock_guard generalLock(this->generalMutex); - std::lock_guard poseLock(this->poseMutex); - - this->DemoteCurrentScenes(); - this->sceneRequestId = -1; - this->SendSceneRequest(); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnLightUpdate(::ConstLightPtr &_lightMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->generalMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnLightUpdate(_lightMsg); - this->newSceneManager->OnLightUpdate(_lightMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnModelUpdate(::ConstModelPtr &_modelMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->generalMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnModelUpdate(_modelMsg); - this->newSceneManager->OnModelUpdate(_modelMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnJointUpdate(::ConstJointPtr &_jointMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->generalMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnJointUpdate(_jointMsg); - this->newSceneManager->OnJointUpdate(_jointMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnVisualUpdate(::ConstVisualPtr &_visualMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->generalMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnVisualUpdate(_visualMsg); - this->newSceneManager->OnVisualUpdate(_visualMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnSensorUpdate(::ConstSensorPtr &_sensorMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->generalMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnSensorUpdate(_sensorMsg); - this->newSceneManager->OnSensorUpdate(_sensorMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->poseMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnPoseUpdate(_posesMsg); - this->newSceneManager->OnPoseUpdate(_posesMsg); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::OnRemovalUpdate(const std::string &_name) -{ - // wait for update unlock before adding message - std::lock_guard lock(this->poseMutex); - - if (this->currentSceneManager == nullptr || this->newSceneManager == nullptr) - { - return; - } - - this->currentSceneManager->OnRemovalUpdate(_name); - this->newSceneManager->OnRemovalUpdate(_name); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::PromoteNewScenes() -{ - unsigned int newSceneCount = this->newSceneManager->SceneCount(); - - // move each new scene - for (unsigned int i = 0; i < newSceneCount; ++i) - { - ScenePtr scene = this->newSceneManager->SceneAt(i); - this->currentSceneManager->AddScene(scene); - } - - // clear new scenes - this->newSceneManager->Clear(); -} - -////////////////////////////////////////////////// -void SceneManagerPrivate::DemoteCurrentScenes() -{ - // promote new first to clear messages - // & to maintain scene index order - this->PromoteNewScenes(); - - // get updated current scene size - unsigned int currSceneCount = this->currentSceneManager->SceneCount(); - - // move each current scene - for (unsigned int i = 0; i < currSceneCount; ++i) - { - ScenePtr scene = this->currentSceneManager->SceneAt(i); - this->newSceneManager->AddScene(scene); - scene->Clear(); - } - - // clear current scenes - this->currentSceneManager->Clear(); -} - -////////////////////////////////////////////////// -SubSceneManager::SubSceneManager() : - activeScene(nullptr) -{ - this->CreateGeometryFunctionMap(); -} - -////////////////////////////////////////////////// -SubSceneManager::~SubSceneManager() -{ -} - -////////////////////////////////////////////////// -unsigned int SubSceneManager::SceneCount() const -{ - // TODO(anyone): encapsulate - - return this->scenes.size(); -} - -////////////////////////////////////////////////// -bool SubSceneManager::HasScene(unsigned int _id) const -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Id() == _id) - { - return true; - } - } - - return false; -} - -////////////////////////////////////////////////// -bool SubSceneManager::HasScene(const std::string &_name) const -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Name() == _name) - { - return true; - } - } - - return false; -} - -////////////////////////////////////////////////// -bool SubSceneManager::HasScene(ConstScenePtr _scene) const -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene == _scene) - { - return true; - } - } - - return false; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::Scene(unsigned int _id) const -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Id() == _id) - { - return scene; - } - } - - return nullptr; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::Scene(const std::string &_name) const -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Name() == _name) - { - return scene; - } - } - - return nullptr; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::SceneAt(unsigned int _index) const -{ - // TODO(anyone): encapsulate - - if (_index >= this->SceneCount()) - { - gzerr << "Invalid scene index: " << _index << std::endl; - return nullptr; - } - - auto iter = this->scenes.begin(); - std::advance(iter, _index); - return *iter; -} - -////////////////////////////////////////////////// -void SubSceneManager::AddScene(ScenePtr _scene) -{ - // TODO(anyone): encapsulate - - if (!_scene) - { - gzerr << "Cannot add null scene pointer" << std::endl; - return; - } - - if (this->HasScene(_scene)) - { - gzerr << "Scene has already been added" << std::endl; - return; - } - - this->scenes.push_back(_scene); -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::RemoveScene(unsigned int _id) -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Id() == _id) - { - return scene; - } - } - - return nullptr; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::RemoveScene(const std::string &_name) -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene->Name() == _name) - { - return scene; - } - } - - return nullptr; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::RemoveScene(ScenePtr _scene) -{ - // TODO(anyone): encapsulate - - for (auto scene : this->scenes) - { - if (scene == _scene) - { - return scene; - } - } - - return nullptr; -} - -////////////////////////////////////////////////// -ScenePtr SubSceneManager::RemoveSceneAt(unsigned int _index) -{ - // TODO(anyone): encapsulate - - if (_index >= this->SceneCount()) - { - gzerr << "Invalid scene index: " << _index << std::endl; - return nullptr; - } - - auto iter = this->scenes.begin(); - std::advance(iter, _index); - ScenePtr scene = *iter; - this->scenes.erase(iter); - return scene; -} - -////////////////////////////////////////////////// -void SubSceneManager::RemoveScenes() -{ - this->Clear(); -} - -////////////////////////////////////////////////// -void SubSceneManager::UpdateScenes() -{ - // update each scene in list - for (auto scene : this->scenes) - { - this->activeScene = scene; - this->ProcessMessages(); - } - - ClearMessages(); - this->activeScene = nullptr; -} - -////////////////////////////////////////////////// -void SubSceneManager::Clear() -{ - this->scenes.clear(); - this->ClearMessages(); - this->activeScene = nullptr; -} - -////////////////////////////////////////////////// -void SubSceneManager::OnLightUpdate(::ConstLightPtr &_lightMsg) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->lightMsgs.push_back(*_lightMsg); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::OnModelUpdate(::ConstModelPtr &_modelMsg) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->modelMsgs.push_back(*_modelMsg); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::OnJointUpdate(::ConstJointPtr &_jointMsg) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->jointMsgs.push_back(*_jointMsg); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::OnVisualUpdate(::ConstVisualPtr &_visualMsg) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->visualMsgs.push_back(*_visualMsg); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::OnSensorUpdate(::ConstSensorPtr &_sensorMsg) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->sensorMsgs.push_back(*_sensorMsg); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::OnRemovalUpdate(const std::string &_name) -{ - // check if message needed - if (!this->scenes.empty()) - { - this->approvedRemovals.push_back(_name); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessMessages() -{ - // process each queued message -//! [process message] - this->ProcessLights(); - this->ProcessModels(); - this->ProcessJoints(); - this->ProcessVisuals(); - this->ProcessSensors(); - this->ProcessPoses(); - this->ProcessRemovals(); -//! [process message] - - // flush changes to scene - this->activeScene->SetTime(this->timePosesReceived); - this->activeScene->PreRender(); -} - -////////////////////////////////////////////////// -void SubSceneManager::ClearMessages() -{ - this->lightMsgs.clear(); - this->modelMsgs.clear(); - this->jointMsgs.clear(); - this->visualMsgs.clear(); - this->sensorMsgs.clear(); - this->approvedRemovals.clear(); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessLight(const gazebo::msgs::Light &_lightMsg) -{ - // TODO(anyone): get parent when protobuf message is updated - this->ProcessLight(_lightMsg, this->activeScene->RootVisual()); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessLight(const gazebo::msgs::Light &_lightMsg, - VisualPtr _parent) -{ - // check if type specified - if (_lightMsg.has_type()) - { - gazebo::msgs::Light::LightType type = _lightMsg.type(); - - // determine light type - switch (_lightMsg.type()) - { - case gazebo::msgs::Light::POINT: - this->ProcessPointLight(_lightMsg, _parent); - return; - - case gazebo::msgs::Light::SPOT: - this->ProcessSpotLight(_lightMsg, _parent); - return; - - case gazebo::msgs::Light::DIRECTIONAL: - this->ProcessDirectionalLight(_lightMsg, _parent); - return; - - default: - gzerr << "Invalid light type: " << type << std::endl; - return; - } - } - - // update existing light - std::string name = _lightMsg.name(); - LightPtr light = this->activeScene->LightByName(name); - if (light) this->ProcessLightImpl(_lightMsg, light); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessDirectionalLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent) -{ - DirectionalLightPtr light = this->DirectionalLight(_lightMsg, _parent); - if (light) this->ProcessDirectionalLightImpl(_lightMsg, light); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessDirectionalLightImpl( - const gazebo::msgs::Light &_lightMsg, DirectionalLightPtr _light) -{ - // set direction if available - if (_lightMsg.has_direction()) - { - const gazebo::msgs::Vector3d &dirMsg = _lightMsg.direction(); - _light->SetDirection(SubSceneManager::Convert(dirMsg)); - } - - // process general light information - this->ProcessLightImpl(_lightMsg, _light); -} - -////////////////////////////////////////////////// -DirectionalLightPtr SubSceneManager::DirectionalLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent) -{ - // find existing light with name - std::string name = _lightMsg.name(); - LightPtr light = this->activeScene->LightByName(name); - - DirectionalLightPtr dirLight = - std::dynamic_pointer_cast(light); - - // check if not found - if (!dirLight) - { - dirLight = this->CreateDirectionalLight(_lightMsg); - _parent->AddChild(dirLight); - } - - return dirLight; -} - -////////////////////////////////////////////////// -DirectionalLightPtr SubSceneManager::CreateDirectionalLight( - const gazebo::msgs::Light &_lightMsg) -{ - std::string name = _lightMsg.name(); - return this->activeScene->CreateDirectionalLight(name); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessPointLight(const gazebo::msgs::Light &_lightMsg, - VisualPtr _parent) -{ - PointLightPtr light = this->PointLight(_lightMsg, _parent); - if (light) this->ProcessPointLightImpl(_lightMsg, light); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessPointLightImpl( - const gazebo::msgs::Light &_lightMsg, PointLightPtr _light) -{ - // process general light information - this->ProcessLightImpl(_lightMsg, _light); -} - -////////////////////////////////////////////////// -PointLightPtr SubSceneManager::PointLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent) -{ - // find existing light with name - std::string name = _lightMsg.name(); - LightPtr light = this->activeScene->LightByName(name); - PointLightPtr pointLight = - std::dynamic_pointer_cast(light); - - // check if not found - if (!pointLight) - { - pointLight = this->CreatePointLight(_lightMsg); - _parent->AddChild(pointLight); - } - - return pointLight; -} - -////////////////////////////////////////////////// -PointLightPtr SubSceneManager::CreatePointLight( - const gazebo::msgs::Light &_lightMsg) -{ - std::string name = _lightMsg.name(); - return this->activeScene->CreatePointLight(name); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessSpotLight(const gazebo::msgs::Light &_lightMsg, - VisualPtr _parent) -{ - SpotLightPtr light = this->SpotLight(_lightMsg, _parent); - if (light) this->ProcessSpotLightImpl(_lightMsg, light); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessSpotLightImpl( - const gazebo::msgs::Light &_lightMsg, SpotLightPtr _light) -{ - // set direction if available - if (_lightMsg.has_direction()) - { - const gazebo::msgs::Vector3d &dirMsg = _lightMsg.direction(); - _light->SetDirection(SubSceneManager::Convert(dirMsg)); - } - - // set inner-angle if available - if (_lightMsg.has_spot_inner_angle()) - { - double radians = _lightMsg.spot_inner_angle(); - _light->SetInnerAngle(math::Angle(radians)); - } - - // set outer-angle if available - if (_lightMsg.has_spot_outer_angle()) - { - double radians = _lightMsg.spot_outer_angle(); - _light->SetOuterAngle(math::Angle(radians)); - } - - // set falloff if available - if (_lightMsg.has_spot_falloff()) - { - double falloff = _lightMsg.spot_falloff(); - _light->SetFalloff(falloff); - } - - // process general light information - this->ProcessLightImpl(_lightMsg, _light); -} - -////////////////////////////////////////////////// -SpotLightPtr SubSceneManager::SpotLight(const gazebo::msgs::Light &_lightMsg, - VisualPtr _parent) -{ - // find existing light with name - std::string name = _lightMsg.name(); - LightPtr light = this->activeScene->LightByName(name); - SpotLightPtr spotLight = - std::dynamic_pointer_cast(light); - - // check if not found - if (!spotLight) - { - spotLight = this->CreateSpotLight(_lightMsg); - _parent->AddChild(spotLight); - } - - return spotLight; -} - -////////////////////////////////////////////////// -SpotLightPtr SubSceneManager::CreateSpotLight( - const gazebo::msgs::Light &_lightMsg) -{ - std::string name = _lightMsg.name(); - return this->activeScene->CreateSpotLight(name); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessLightImpl(const gazebo::msgs::Light &_lightMsg, - LightPtr _light) -{ - // set pose if available - if (_lightMsg.has_pose()) - { - this->SetPose(_light, _lightMsg.pose()); - } - - // set diffuse if available - if (_lightMsg.has_diffuse()) - { - const gazebo::msgs::Color &colorMsg = _lightMsg.diffuse(); - _light->SetDiffuseColor(SubSceneManager::Convert(colorMsg)); - } - - // set specular if available - if (_lightMsg.has_specular()) - { - const gazebo::msgs::Color &colorMsg = _lightMsg.specular(); - _light->SetSpecularColor(SubSceneManager::Convert(colorMsg)); - } - - // set attenuation constant if available - if (_lightMsg.has_attenuation_constant()) - { - double attenConst = _lightMsg.attenuation_constant(); - _light->SetAttenuationConstant(attenConst); - } - - // set attenuation linear if available - if (_lightMsg.has_attenuation_linear()) - { - double attenLinear = _lightMsg.attenuation_linear(); - _light->SetAttenuationLinear(attenLinear); - } - - // set attenuation quadratic if available - if (_lightMsg.has_attenuation_quadratic()) - { - double attenQuad = _lightMsg.attenuation_quadratic(); - _light->SetAttenuationQuadratic(attenQuad); - } - - // set attenuation range if available - if (_lightMsg.has_range()) - { - double attenRange = _lightMsg.range(); - _light->SetAttenuationRange(attenRange); - } - - // set cast-shadows if available - if (_lightMsg.has_cast_shadows()) - { - bool castShadows = _lightMsg.cast_shadows(); - _light->SetCastShadows(castShadows); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessSensor(const gazebo::msgs::Sensor &_sensorMsg) -{ - VisualPtr parent = this->Parent(_sensorMsg.parent()); - this->ProcessSensor(_sensorMsg, parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessSensor(const gazebo::msgs::Sensor &_sensorMsg, - VisualPtr _parent) -{ - // TODO(anyone): process all types - - if (_sensorMsg.has_camera()) - { - this->ProcessCamera(_sensorMsg, _parent); - } -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessCamera(const gazebo::msgs::Sensor &_sensorMsg, - VisualPtr _parent) -{ - CameraPtr camera = this->Camera(_sensorMsg, _parent); - - // TODO(anyone): update camera -} - -////////////////////////////////////////////////// -CameraPtr SubSceneManager::Camera(const gazebo::msgs::Sensor &_sensorMsg, - VisualPtr _parent) -{ - // find existing camera with name - std::string name = _sensorMsg.name(); - SensorPtr sensor = this->activeScene->SensorByName(name); - CameraPtr camera = std::dynamic_pointer_cast(sensor); - - // check if not found - if (!camera) - { - camera = this->CreateCamera(_sensorMsg); - _parent->AddChild(camera); - } - - return camera; -} - -////////////////////////////////////////////////// -CameraPtr SubSceneManager::CreateCamera(const gazebo::msgs::Sensor &_sensorMsg) -{ - bool hasId = _sensorMsg.has_id(); - unsigned int id = _sensorMsg.id(); - std::string name = _sensorMsg.name(); - - return (hasId) ? - this->activeScene->CreateCamera(id, name) : - this->activeScene->CreateCamera(name); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessModel(const gazebo::msgs::Model &_modelMsg) -{ - VisualPtr parent = this->activeScene->RootVisual(); - this->ProcessModel(_modelMsg, parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessModel(const gazebo::msgs::Model &_modelMsg, - VisualPtr _parent) -{ - VisualPtr model = this->Model(_modelMsg, _parent); - - // set pose if available - if (_modelMsg.has_pose()) - { - this->SetPose(model, _modelMsg.pose()); - } - - // set scale if available - if (_modelMsg.has_scale()) - { - this->SetScale(model, _modelMsg.scale()); - } - - // process each sensor in joint - for (int i = 0; i < _modelMsg.joint_size(); ++i) - { - const gazebo::msgs::Joint &joint = _modelMsg.joint(i); - this->ProcessJoint(joint, model); - } - - // process each sensor in link - for (int i = 0; i < _modelMsg.link_size(); ++i) - { - const gazebo::msgs::Link &link = _modelMsg.link(i); - this->ProcessLink(link, model); - } - - // process each sensor in visual - // always skip first empty visual - for (int i = 1; i < _modelMsg.visual_size(); ++i) - { - const gazebo::msgs::Visual &visual = _modelMsg.visual(i); - this->ProcessVisual(visual, model); - } -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Model(const gazebo::msgs::Model &_modelMsg, - VisualPtr _parent) -{ - bool hasId = _modelMsg.has_id(); - unsigned int id = _modelMsg.id(); - std::string name = _modelMsg.name(); - return this->Visual(hasId, id, name, _parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessJoint(const gazebo::msgs::Joint &_jointMsg) -{ - VisualPtr parent = this->Parent(_jointMsg.parent()); - this->ProcessJoint(_jointMsg, parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessJoint(const gazebo::msgs::Joint &_jointMsg, - VisualPtr _parent) -{ - VisualPtr joint = this->Joint(_jointMsg, _parent); - - // set pose if available - if (_jointMsg.has_pose()) - { - this->SetPose(joint, _jointMsg.pose()); - } - - // process each sensor in joint - for (int i = 0; i < _jointMsg.sensor_size(); ++i) - { - const gazebo::msgs::Sensor &sensor = _jointMsg.sensor(i); - this->ProcessSensor(sensor, joint); - } -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Joint(const gazebo::msgs::Joint &_jointMsg, - VisualPtr _parent) -{ - bool hasId = _jointMsg.has_id(); - unsigned int id = _jointMsg.id(); - std::string name = _jointMsg.name(); - return this->Visual(hasId, id, name, _parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessVisual(const gazebo::msgs::Visual &_visualMsg) -{ - VisualPtr parent = this->Parent(_visualMsg.parent_name()); - this->ProcessVisual(_visualMsg, parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessVisual(const gazebo::msgs::Visual &_visualMsg, - VisualPtr _parent) -{ - VisualPtr visual = this->Visual(_visualMsg, _parent); - - // TODO(anyone): handle cast shadows - // TODO(anyone): handle transparency - // TODO(anyone): handle scale & geom size - - // set pose if available - if (_visualMsg.has_pose()) - { - this->SetPose(visual, _visualMsg.pose()); - } - - // set scale if available - if (_visualMsg.has_scale()) - { - this->SetScale(visual, _visualMsg.scale()); - } - - // set geometry if available - if (_visualMsg.has_geometry()) - { - this->ProcessGeometry(_visualMsg.geometry(), visual); - } - - // set material if available - if (_visualMsg.has_material()) - { - const gazebo::msgs::Material matMsg = _visualMsg.material(); - MaterialPtr material = this->CreateMaterial(matMsg); - visual->SetMaterial(material); - } -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Visual(const gazebo::msgs::Visual &_visualMsg, - VisualPtr _parent) -{ - bool hasId = _visualMsg.has_id(); - unsigned int id = _visualMsg.id(); - std::string name = _visualMsg.name(); - return this->Visual(hasId, id, name, _parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessLink(const gazebo::msgs::Link &_linkMsg, - VisualPtr _parent) -{ - VisualPtr link = this->Link(_linkMsg, _parent); - - // set pose if available - if (_linkMsg.has_pose()) - { - this->SetPose(link, _linkMsg.pose()); - } - - // process each visual in link - // always skip first empty visual - for (int i = 1; i < _linkMsg.visual_size(); ++i) - { - const gazebo::msgs::Visual &visual = _linkMsg.visual(i); - this->ProcessVisual(visual, link); - } - - // process each sensor in link - for (int i = 0; i < _linkMsg.sensor_size(); ++i) - { - const gazebo::msgs::Sensor &sensor = _linkMsg.sensor(i); - this->ProcessSensor(sensor, link); - } -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Link(const gazebo::msgs::Link &_linkMsg, - VisualPtr _parent) -{ - bool hasId = _linkMsg.has_id(); - unsigned int id = _linkMsg.id(); - std::string name = _linkMsg.name(); - return this->Visual(hasId, id, name, _parent); -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Visual(bool _hasId, unsigned int _id, - const std::string &_name, VisualPtr _parent) -{ - // find existing visual with name - VisualPtr visual = this->activeScene->VisualByName(_name); - - // check if not found - if (!visual) - { - visual = this->CreateVisual(_hasId, _id, _name); - _parent->AddChild(visual); - } - - return visual; -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::CreateVisual(bool _hasId, unsigned int _id, - const std::string &_name) -{ - return (_hasId) ? - this->activeScene->CreateVisual(_id, _name) : - this->activeScene->CreateVisual(_name); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessGeometry( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent) -{ - GeomType geomType = _geometryMsg.type(); - GeomFunc geomFunc = this->geomFunctions[geomType]; - _parent->RemoveGeometries(); - - // check if invalid type - if (!geomFunc) - { - gzerr << "Unsupported geometry type: " << geomType << std::endl; - gzwarn << "Using empty geometry instead" << std::endl; - geomFunc = this->geomFunctions[gazebo::msgs::Geometry::EMPTY]; - } - - (this->*geomFunc)(_geometryMsg, _parent); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessBox(const gazebo::msgs::Geometry &_geometryMsg, - VisualPtr _parent) -{ - GeometryPtr box = this->activeScene->CreateBox(); - const gazebo::msgs::BoxGeom &boxMsg = _geometryMsg.box(); - const gazebo::msgs::Vector3d sizeMsg = boxMsg.size(); - _parent->SetLocalScale(SubSceneManager::Convert(sizeMsg)); - _parent->AddGeometry(box); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessCone( - const gazebo::msgs::Geometry &/*_geometryMsg*/, VisualPtr _parent) -{ - // TODO(anyone): needs protobuf msg - GeometryPtr cone = this->activeScene->CreateCone(); - // const gazebo::msgs::ConeGeom &coneMsg = _geometryMsg.cone(); - // double x = coneMsg.radius(); - // double y = coneMsg.radius(); - // double z = coneMsg.length(); - // _parent->SetLocalScale(x, y, z); - _parent->AddGeometry(cone); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessCapsule( - const gazebo::msgs::Geometry & _geometryMsg, VisualPtr _parent) -{ - // \todo(anyone) needs gazebo capsule msg - CapsulePtr capsule = this->activeScene->CreateCapsule(); - // Const gazebo::msgs::CapsuleGeom &capsuleMsg = _geometryMsg.capsule(); - // Double x = 2 * capsuleMsg.radius(); - // Double y = 2 * capsuleMsg.radius(); - // Double z = capsuleMsg.length(); - // _parent->SetLocalScale(x, y, z); - _parent->AddGeometry(std::dynamic_pointer_cast(capsule)); -} - -////////////////////////////////////////////////// -//! [process cylinder] -void SubSceneManager::ProcessCylinder( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent) -{ - GeometryPtr cylinder = this->activeScene->CreateCylinder(); - const gazebo::msgs::CylinderGeom &cylinderMsg = _geometryMsg.cylinder(); - double x = 2 * cylinderMsg.radius(); - double y = 2 * cylinderMsg.radius(); - double z = cylinderMsg.length(); - _parent->SetLocalScale(x, y, z); - _parent->AddGeometry(cylinder); -} -//! [process cylinder] - -////////////////////////////////////////////////// -void SubSceneManager::ProcessEmpty(const gazebo::msgs::Geometry&, VisualPtr) -{ - // do nothing -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessMesh(const gazebo::msgs::Geometry &_geometryMsg, - VisualPtr _parent) -{ - const gazebo::msgs::MeshGeom &meshMsg = _geometryMsg.mesh(); - - // if the model contains model:// try to find the meshes in ~/.gazebo - std::list modelPaths; - char *homePath = getenv("HOME"); - std::string home; - if (!homePath) - home = "/tmp/gazebo"; - else - home = homePath; - - modelPaths.push_back(home + "/.gazebo/models"); - - std::string meshName = meshMsg.filename(); - int index = meshName.find("://"); - std::string prefix = meshName.substr(0, index); - std::string suffix = meshName.substr(index + 3, meshName.size() - index - 3); - - if (prefix == "model") - { - fs::path path; - for (std::list::iterator iter = modelPaths.begin(); - iter != modelPaths.end(); ++iter) - { - path = fs::path(*iter) / suffix; - if (fs::exists(path)) - { - meshName = path.string(); - break; - } - } - } - - // initialize mesh parameters - MeshDescriptor descriptor; - descriptor.meshName = meshName; - - // assign sub-mesh if available - if (meshMsg.has_submesh()) - { - descriptor.subMeshName = meshMsg.submesh(); - } - - // assign sub-mesh if available - if (meshMsg.has_center_submesh()) - { - descriptor.centerSubMesh = meshMsg.center_submesh(); - } - - // actually create mesh geometry - common::MeshManager *meshManager = common::MeshManager::Instance(); - descriptor.mesh = meshManager->Load(descriptor.meshName); - MeshPtr mesh = this->activeScene->CreateMesh(descriptor); - - // set scale if available - if (meshMsg.has_scale()) - { - const gazebo::msgs::Vector3d scaleMsg = meshMsg.scale(); - _parent->SetLocalScale(SubSceneManager::Convert(scaleMsg)); - } - - // attach geometry to parent - _parent->AddGeometry(mesh); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessPlane(const gazebo::msgs::Geometry &_geometryMsg, - VisualPtr _parent) -{ - // TODO(anyone): handle plane normal - GeometryPtr plane = this->activeScene->CreatePlane(); - const gazebo::msgs::PlaneGeom &planeMsg = _geometryMsg.plane(); - const gazebo::msgs::Vector2d planeSize = planeMsg.size(); - _parent->SetLocalScale(planeSize.x(), planeSize.y(), 1); - _parent->AddGeometry(plane); -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessSphere(const gazebo::msgs::Geometry &_geometryMsg, - VisualPtr _parent) -{ - GeometryPtr sphere = this->activeScene->CreateSphere(); - const gazebo::msgs::SphereGeom &sphereMsg = _geometryMsg.sphere(); - _parent->SetLocalScale(2 * sphereMsg.radius()); - _parent->AddGeometry(sphere); -} - -////////////////////////////////////////////////// -MaterialPtr SubSceneManager::CreateMaterial( - const gazebo::msgs::Material &_materialMsg) -{ - MaterialPtr material = this->activeScene->CreateMaterial(); - - // TODO(anyone): remove after testing - material->SetShininess(50); - material->SetReflectivity(0.25); - - // set ambient if available - if (_materialMsg.has_ambient()) - { - gazebo::msgs::Color msg = _materialMsg.ambient(); - math::Color ambient(msg.r(), msg.g(), msg.b(), msg.a()); - material->SetAmbient(ambient); - } - - // set diffuse if available - if (_materialMsg.has_diffuse()) - { - gazebo::msgs::Color msg = _materialMsg.diffuse(); - math::Color diffuse(msg.r(), msg.g(), msg.b(), msg.a()); - material->SetDiffuse(diffuse); - } - - // set specular if available - if (_materialMsg.has_specular()) - { - gazebo::msgs::Color msg = _materialMsg.specular(); - math::Color specular(msg.r(), msg.g(), msg.b(), msg.a()); - material->SetSpecular(specular); - } - - // set emissive if available - if (_materialMsg.has_emissive()) - { - gazebo::msgs::Color msg = _materialMsg.emissive(); - math::Color emissive(msg.r(), msg.g(), msg.b(), msg.a()); - material->SetEmissive(emissive); - } - - // set lighting if available - if (_materialMsg.has_lighting()) - { - bool lighting = _materialMsg.lighting(); - material->SetLightingEnabled(lighting); - } - - // set normal-map if available - if (_materialMsg.has_normal_map()) - { - const std::string &normal_map = _materialMsg.normal_map(); - material->SetNormalMap(normal_map); - } - - // set shader-type if available - if (_materialMsg.has_shader_type()) - { - gazebo::msgs::Material::ShaderType shader_type = - _materialMsg.shader_type(); - ShaderType type = SubSceneManager::Convert(shader_type); - material->SetShaderType(type); - } - - // TODO(anyone): handle scripts - - return material; -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessPose(const gazebo::msgs::Pose &_poseMsg) -{ - std::string name = _poseMsg.name(); - NodePtr node = this->activeScene->NodeByName(name); - if (node) this->SetPose(node, _poseMsg); -} - -////////////////////////////////////////////////// -void SubSceneManager::SetPose(NodePtr _node, - const gazebo::msgs::Pose &_poseMsg) -{ - math::Pose3d pose = SubSceneManager::Convert(_poseMsg); - _node->SetLocalPose(pose); -} - -////////////////////////////////////////////////// -void SubSceneManager::SetScale(VisualPtr _visual, - const gazebo::msgs::Vector3d &_scaleMsg) -{ - math::Vector3d scale = SubSceneManager::Convert(_scaleMsg); - _visual->SetLocalScale(scale); -} - -////////////////////////////////////////////////// -VisualPtr SubSceneManager::Parent(const std::string &_name) -{ - // assign default parent node - VisualPtr parent = this->activeScene->RootVisual(); - - // check if name given - if (!_name.empty()) - { - // get node with name - parent = this->activeScene->VisualByName(_name); - - // node not found - if (!parent) - { - if (_name != "default") - { - gzerr << "invalid parent name: " << _name << std::endl; - gzwarn << "using scene root node" << std::endl; - } - - parent = this->activeScene->RootVisual(); - } - } - - return parent; -} - -////////////////////////////////////////////////// -void SubSceneManager::ProcessRemoval(const std::string &_name) -{ - this->activeScene->DestroyNodeByName(_name); -} - -////////////////////////////////////////////////// -math::Color SubSceneManager::Convert(const gazebo::msgs::Color &_colorMsg) -{ - math::Color color; - color.R() = _colorMsg.r(); - color.G() = _colorMsg.g(); - color.B() = _colorMsg.b(); - color.A() = _colorMsg.a(); - return color; -} - -////////////////////////////////////////////////// -math::Pose3d SubSceneManager::Convert(const gazebo::msgs::Pose &_poseMsg) -{ - math::Pose3d pose; - - const gazebo::msgs::Vector3d &posMsg = _poseMsg.position(); - pose.Pos() = SubSceneManager::Convert(posMsg); - - const gazebo::msgs::Quaternion &rotMsg = _poseMsg.orientation(); - pose.Rot() = SubSceneManager::Convert(rotMsg); - - return pose; -} - -////////////////////////////////////////////////// -math::Vector3d SubSceneManager::Convert(const gazebo::msgs::Vector3d &_vecMsg) -{ - math::Vector3d vec; - vec.X(_vecMsg.x()); - vec.Y(_vecMsg.y()); - vec.Z(_vecMsg.z()); - return vec; -} - -////////////////////////////////////////////////// -math::Quaterniond SubSceneManager::Convert( - const gazebo::msgs::Quaternion &_quatMsg) -{ - math::Quaterniond quat; - quat.W(_quatMsg.w()); - quat.X(_quatMsg.x()); - quat.Y(_quatMsg.y()); - quat.Z(_quatMsg.z()); - return quat; -} - -////////////////////////////////////////////////// -ShaderType SubSceneManager::Convert(gazebo::msgs::Material::ShaderType _type) -{ - switch (_type) - { - case gazebo::msgs::Material::VERTEX: - return ST_VERTEX; - - case gazebo::msgs::Material::PIXEL: - return ST_PIXEL; - - case gazebo::msgs::Material::NORMAL_MAP_OBJECT_SPACE: - return ST_NORM_OBJ; - - case gazebo::msgs::Material::NORMAL_MAP_TANGENT_SPACE: - return ST_NORM_TAN; - - default: - return ST_UNKNOWN; - } -} - -////////////////////////////////////////////////// -void SubSceneManager::CreateGeometryFunctionMap() -{ - this->geomFunctions[gazebo::msgs::Geometry::BOX] = - &SubSceneManager::ProcessBox; - - // todo(anyone): enable when cone protobuf msg is created - // this->geomFunctions[gazebo::msgs::Geometry::CONE] = - // &SubSceneManager::ProcessCone; - - // todo(anyone): enable when capsule protobuf msg is created - // this->geomFunctions[gazebo::msgs::Geometry::CAPSULE] = - // &SubSceneManager::ProcessSphere; - - this->geomFunctions[gazebo::msgs::Geometry::CYLINDER] = - &SubSceneManager::ProcessCylinder; - - this->geomFunctions[gazebo::msgs::Geometry::EMPTY] = - &SubSceneManager::ProcessEmpty; - - this->geomFunctions[gazebo::msgs::Geometry::MESH] = - &SubSceneManager::ProcessMesh; - - this->geomFunctions[gazebo::msgs::Geometry::PLANE] = - &SubSceneManager::ProcessPlane; - - this->geomFunctions[gazebo::msgs::Geometry::SPHERE] = - &SubSceneManager::ProcessSphere; -} - -////////////////////////////////////////////////// -CurrentSceneManager::CurrentSceneManager() -{ -} - -////////////////////////////////////////////////// -CurrentSceneManager::~CurrentSceneManager() -{ -} - -////////////////////////////////////////////////// -//! [on update pose] -void CurrentSceneManager::OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg) -{ - // record pose timestamp - this->timePosesReceived = std::chrono::seconds(_posesMsg->time().sec()) + - std::chrono::nanoseconds(_posesMsg->time().nsec()); - - // process each pose in message - for (int i = 0; i < _posesMsg->pose_size(); ++i) - { - // replace into pose map - gazebo::msgs::Pose pose = _posesMsg->pose(i); - std::string name = pose.name(); - this->poseMsgs[name] = pose; - } -} -//! [on update pose] - -////////////////////////////////////////////////// -void CurrentSceneManager::ClearMessages() -{ - SubSceneManager::ClearMessages(); - this->poseMsgs.clear(); -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessLights() -{ - // process each light in list - for (auto &lightMsg : this->lightMsgs) - { - this->ProcessLight(lightMsg); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessModels() -{ - // process each model in list - for (auto &modelMsg : this->modelMsgs) - { - this->ProcessModel(modelMsg); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessJoints() -{ - // process each joint in list - for (auto &jointMsg : this->jointMsgs) - { - this->ProcessJoint(jointMsg); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessVisuals() -{ - // process each visual in list - for (auto &visualMsg : this->visualMsgs) - { - this->ProcessVisual(visualMsg); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessSensors() -{ - // process each sensor in list - for (auto &sensorMsg : this->sensorMsgs) - { - this->ProcessSensor(sensorMsg); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessPoses() -{ - // process each pose in list - for (auto poseMsgPair : this->poseMsgs) - { - this->ProcessPose(poseMsgPair.second); - } -} - -////////////////////////////////////////////////// -void CurrentSceneManager::ProcessRemovals() -{ - // process each removal in list - for (auto &removal : this->approvedRemovals) - { - this->ProcessRemoval(removal); - } -} - -////////////////////////////////////////////////// -NewSceneManager::NewSceneManager() : - sceneReceived(false) -{ -} - -////////////////////////////////////////////////// -NewSceneManager::~NewSceneManager() -{ -} - -////////////////////////////////////////////////// -void NewSceneManager::SetSceneData(const std::string &_data) -{ - this->sceneMsg.ParseFromString(_data); - this->sceneReceived = true; -} - -////////////////////////////////////////////////// -void NewSceneManager::OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg) -{ - this->posesMsgs.push_back(*_posesMsg); -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessMessages() -{ - this->ProcessScene(); - SubSceneManager::ProcessMessages(); -} - -////////////////////////////////////////////////// -void NewSceneManager::ClearMessages() -{ - this->posesMsgs.clear(); - this->sceneReceived = false; - SubSceneManager::ClearMessages(); -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessScene() -{ - // TODO(anyone): process environment info - - // delete all previous nodes - // this->activeScene->Clear(); - - // process ambient if available - if (this->sceneMsg.has_ambient()) - { - gazebo::msgs::Color colorMsg = this->sceneMsg.ambient(); - math::Color color(colorMsg.r(), colorMsg.g(), colorMsg.b()); - this->activeScene->SetAmbientLight(color); - } - - // process background if available - if (this->sceneMsg.has_background()) - { - gazebo::msgs::Color colorMsg = this->sceneMsg.background(); - math::Color color(colorMsg.r(), colorMsg.g(), colorMsg.b()); - this->activeScene->SetBackgroundColor(color); - } - - // process each scene light - for (int i = 0; i < this->sceneMsg.light_size(); ++i) - { - gazebo::msgs::Light lightMsg = this->sceneMsg.light(i); - this->ProcessLight(lightMsg, this->activeScene->RootVisual()); - } - - // process each scene model - for (int i = 0; i < this->sceneMsg.model_size(); ++i) - { - gazebo::msgs::Model modelMsg = this->sceneMsg.model(i); - this->ProcessModel(modelMsg, this->activeScene->RootVisual()); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessLights() -{ - // process each light in list - for (auto &lightMsg : this->lightMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessLight(lightMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessModels() -{ - // process each model in list - for (auto &modelMsg : this->modelMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessModel(modelMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessJoints() -{ - // process each joint in list - for (auto &jointMsg : this->jointMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessJoint(jointMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessVisuals() -{ - // process each visual in list - for (auto &visualMsg : this->visualMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessVisual(visualMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessSensors() -{ - // process each sensor in list - for (auto &sensorMsg : this->sensorMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessSensor(sensorMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessPoses() -{ - // process each poses in list - for (auto &posesMsg : this->posesMsgs) - { - // TODO(anyone): check if message sent after scene response - this->ProcessPoses(posesMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessPoses(const gazebo::msgs::PosesStamped &_posesMsg) -{ - // record pose timestamp - this->timePosesReceived = std::chrono::seconds(_posesMsg.time().sec()) + - std::chrono::nanoseconds(_posesMsg.time().nsec()); - - // process each pose in list - for (int i = 0; i < _posesMsg.pose_size(); ++i) - { - gazebo::msgs::Pose poseMsg = _posesMsg.pose(i); - this->ProcessPose(poseMsg); - } -} - -////////////////////////////////////////////////// -void NewSceneManager::ProcessRemovals() -{ - // process each removal in list - for (auto &removal : this->approvedRemovals) - { - // TODO(anyone): check if message sent after scene response - this->ProcessRemoval(removal); - } -} diff --git a/examples/gazebo_scene_viewer/SceneManager.hh b/examples/gazebo_scene_viewer/SceneManager.hh deleted file mode 100644 index 4839f3bb0..000000000 --- a/examples/gazebo_scene_viewer/SceneManager.hh +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Copyright (C) 2015 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 GZ_RENDERING_EXAMPLES_SCENEMANAGER_HH -#define GZ_RENDERING_EXAMPLES_SCENEMANAGER_HH - -#if __cplusplus > 201703L - #include - using fs = std::filesystem; -#else - #include - using namespace std; - namespace fs = std::experimental::filesystem::v1; -#endif - -#include -#include -#include "gz/rendering/RenderTypes.hh" - -namespace gz -{ - namespace rendering - { - /// \class SceneManager SceneManager.hh gz/rendering/SceneManager.hh - /// \brief Manages a collection of scenes. The class provides a single - /// interface for modifications, allowing multiple scenes to stay - /// synchronized. This class currently consumes Gazebo-specific protobuf - /// messages, but will be updated to be Gazebo-agnostic in the future. - class SceneManager : - public virtual common::SingletonT - { - /// \brief Constructor - public: SceneManager(); - - /// \brief Destructor - public: ~SceneManager(); - - /// \brief Load resources - public: void Load(); - - /// \brief Initialize manager - public: void Init(); - - /// \brief Destroy manager - public: void Fini(); - - /// \brief Get number of managed scenes - /// \return The number of managed scenes - public: unsigned int SceneCount() const; - - /// \brief Determine if a scene with the given ID exists - /// \param[in] _id ID of the scene in question - /// \return True if the specified scene is exists - public: bool HasScene(unsigned int _id) const; - - /// \brief Determine if a scene with the given name exists - /// \param[in] _name Name of the scene in question - /// \return True if the specified scene is exists - public: bool HasScene(const std::string &_name) const; - - /// \brief Determine if the given scene exists - /// \param[in] _scene Scene in question - /// \return True if the specified scene is exists - public: bool HasScene(ConstScenePtr _scene) const; - - /// \brief Get scene with the given ID. If no scene exists with the given - /// ID, NULL will be returned. - /// \param[in] _id ID of the desired scene - /// \return The specified scene - public: ScenePtr Scene(unsigned int _id) const; - - /// \brief Get scene with the given name. If no scene exists with the - /// given name, NULL will be returned. - /// \param[in] _id ID of the desired scene - /// \return The specified scene - public: ScenePtr Scene(const std::string &_name) const; - - /// \brief Get scene at the given index. If no scene exists at the - /// given index, NULL will be returned. - /// \param[in] _index Index of the desired scene - /// \return The specified scene - public: ScenePtr SceneAt(unsigned int _index) const; - - /// \brief Add the given scene. If the given scene has already been added - /// then no work will be done. - /// \param[in] _scene Scene to be added - public: void AddScene(ScenePtr _scene); - - /// \brief Remove the scene with the given ID. If not scene exists with - /// the given ID, then no work will be done. - /// \param[in] _id ID of the scene to be removed - /// \return The removed scene - public: ScenePtr RemoveScene(unsigned int _id); - - /// \brief Remove the scene with the given name. If not scene exists with - /// the given name, then no work will be done. - /// \param[in] _name Name of the scene to be removed - /// \return The removed scene - public: ScenePtr RemoveScene(const std::string &_name); - - /// \brief Remove the given scene. If the given scene has not yet been - /// added, then no work will be done. - /// \param[in] _scene Scene to be removed - /// \return The removed scene - public: ScenePtr RemoveScene(ScenePtr _scene); - - /// \brief Remove the scene at the given index. If not scene exists at - /// the given index, then no work will be done. - /// \param[in] _index Index of the scene to be removed - /// \return The removed scene - public: ScenePtr RemoveSceneAt(unsigned int _index); - - /// \brief Remove all scenes - public: void RemoveScenes(); - - /// \brief Update all scenes - public: void UpdateScenes(); - - /// \brief Private implementation pointer - private: class SceneManagerPrivate *pimpl; - - /// \brief Required SingletonT friendship - private: friend class SingletonT; - }; - } -} -#endif diff --git a/examples/gazebo_scene_viewer/SceneManagerPrivate.hh b/examples/gazebo_scene_viewer/SceneManagerPrivate.hh deleted file mode 100644 index e916fc042..000000000 --- a/examples/gazebo_scene_viewer/SceneManagerPrivate.hh +++ /dev/null @@ -1,473 +0,0 @@ -/* - * Copyright (C) 2015 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 GZ_RENDERING_EXAMPLES_SCENEMANAGERPRIVATE_HH -#define GZ_RENDERING_EXAMPLES_SCENEMANAGERPRIVATE_HH - -#include -#include -#include -#include - -#include - -#include -#include - -#include "gz/rendering/RenderTypes.hh" -#include "gz/rendering/ShaderType.hh" -#include "gazebo/transport/Node.hh" - -namespace gz -{ - namespace rendering - { - class CurrentSceneManager; - - class NewSceneManager; - - class SceneManagerPrivate - { - private: typedef std::map RemovalMap; - - public: SceneManagerPrivate(); - - public: ~SceneManagerPrivate(); - - public: void Load(); - - public: void Init(); - - public: void Fini(); - - public: unsigned int SceneCount() const; - - public: bool HasScene(unsigned int _id) const; - - public: bool HasScene(const std::string &_name) const; - - public: bool HasScene(ConstScenePtr _scene) const; - - public: ScenePtr Scene(unsigned int _id) const; - - public: ScenePtr Scene(const std::string &_name) const; - - public: ScenePtr SceneAt(unsigned int _index) const; - - public: void AddScene(ScenePtr _scene); - - public: ScenePtr RemoveScene(unsigned int _id); - - public: ScenePtr RemoveScene(const std::string &_name); - - public: ScenePtr RemoveScene(ScenePtr _scene); - - public: ScenePtr RemoveSceneAt(unsigned int _index); - - public: void RemoveScenes(); - - public: void UpdateScenes(); - - public: void OnRequest(::ConstRequestPtr &_requestMsg); - - public: void OnResponse(::ConstResponsePtr &_responseMsg); - - public: void OnSceneResponse(::ConstResponsePtr &_responseMsg); - - public: void OnRemovalResponse(::ConstResponsePtr &_responseMsg); - - public: void OnSceneUpdate(::ConstScenePtr _sceneMsg); - - public: void OnLightUpdate(::ConstLightPtr &_lightMsg); - - public: void OnModelUpdate(::ConstModelPtr &_modelMsg); - - public: void OnJointUpdate(::ConstJointPtr &_jointMsg); - - public: void OnVisualUpdate(::ConstVisualPtr &_visualMsg); - - public: void OnSensorUpdate(::ConstSensorPtr &_sensorMsg); - - public: void OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg); - - private: void OnRemovalUpdate(const std::string &_name); - - private: void SendSceneRequest(); - - private: void PromoteNewScenes(); - - private: void DemoteCurrentScenes(); - - private: std::unique_ptr currentSceneManager; - - private: std::unique_ptr newSceneManager; - - private: gazebo::transport::NodePtr transportNode; - - private: gazebo::transport::PublisherPtr requestPub; - - private: gazebo::transport::SubscriberPtr requestSub; - - private: gazebo::transport::SubscriberPtr responseSub; - - private: gazebo::transport::SubscriberPtr lightSub; - - private: gazebo::transport::SubscriberPtr modelSub; - - private: gazebo::transport::SubscriberPtr jointSub; - - private: gazebo::transport::SubscriberPtr visualSub; - - private: gazebo::transport::SubscriberPtr sensorSub; - - private: gazebo::transport::SubscriberPtr poseSub; - - private: gazebo::event::ConnectionPtr preRenderConn; - - private: int sceneRequestId; - - private: bool promotionNeeded; - - private: RemovalMap requestedRemovals; - - private: std::mutex generalMutex; - - private: std::mutex poseMutex; - }; - - class SubSceneManager - { - protected: typedef gazebo::msgs::Geometry::Type GeomType; - - protected: typedef void (SubSceneManager::*GeomFunc) - (const gazebo::msgs::Geometry&, VisualPtr); - - public: SubSceneManager(); - - public: virtual ~SubSceneManager(); - - public: virtual unsigned int SceneCount() const; - - public: virtual bool HasScene(unsigned int _id) const; - - public: virtual bool HasScene(const std::string &_name) const; - - public: virtual bool HasScene(ConstScenePtr _scene) const; - - public: virtual ScenePtr Scene(unsigned int _id) const; - - public: virtual ScenePtr Scene(const std::string &_name) const; - - public: virtual ScenePtr SceneAt(unsigned int _index) const; - - public: virtual void AddScene(ScenePtr _scene); - - public: virtual ScenePtr RemoveScene(unsigned int _id); - - public: virtual ScenePtr RemoveScene(const std::string &_name); - - public: virtual ScenePtr RemoveScene(ScenePtr _scene); - - public: virtual ScenePtr RemoveSceneAt(unsigned int _index); - - public: virtual void RemoveScenes(); - - public: virtual void UpdateScenes(); - - public: virtual void OnLightUpdate(::ConstLightPtr &_lightMsg); - - public: virtual void OnModelUpdate(::ConstModelPtr &_modelMsg); - - public: virtual void OnJointUpdate(::ConstJointPtr &_jointMsg); - - public: virtual void OnVisualUpdate(::ConstVisualPtr &_visualMsg); - - public: virtual void OnSensorUpdate(::ConstSensorPtr &_sensorMsg); - - public: virtual void OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg) = 0; - - public: virtual void OnRemovalUpdate(const std::string &_name); - - public: virtual void Clear(); - - protected: virtual void ProcessMessages(); - - protected: virtual void ClearMessages(); - - protected: virtual void ProcessLights() = 0; - - protected: virtual void ProcessLight( - const gazebo::msgs::Light &_lightMsg); - - public: virtual void ProcessLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual void ProcessDirectionalLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual void ProcessDirectionalLightImpl( - const gazebo::msgs::Light &_lightMsg, DirectionalLightPtr _light); - - protected: virtual DirectionalLightPtr DirectionalLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual DirectionalLightPtr CreateDirectionalLight( - const gazebo::msgs::Light &_lightMsg); - - protected: virtual void ProcessPointLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual void ProcessPointLightImpl( - const gazebo::msgs::Light &_lightMsg, PointLightPtr _light); - - protected: virtual PointLightPtr PointLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual PointLightPtr CreatePointLight( - const gazebo::msgs::Light &_lightMsg); - - protected: virtual void ProcessSpotLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual void ProcessSpotLightImpl( - const gazebo::msgs::Light &_lightMsg, SpotLightPtr _light); - - protected: virtual SpotLightPtr SpotLight( - const gazebo::msgs::Light &_lightMsg, VisualPtr _parent); - - protected: virtual SpotLightPtr CreateSpotLight( - const gazebo::msgs::Light &_lightMsg); - - protected: virtual void ProcessLightImpl( - const gazebo::msgs::Light &_lightMsg, LightPtr _light); - - protected: virtual void ProcessSensors() = 0; - - protected: virtual void ProcessSensor( - const gazebo::msgs::Sensor &_sensorMsg); - - protected: virtual void ProcessSensor( - const gazebo::msgs::Sensor &_sensorMsg, VisualPtr _parent); - - protected: virtual void ProcessCamera( - const gazebo::msgs::Sensor &_sensorMsg, VisualPtr _parent); - - protected: virtual CameraPtr Camera( - const gazebo::msgs::Sensor &_sensorMsg, VisualPtr _parent); - - protected: virtual CameraPtr CreateCamera( - const gazebo::msgs::Sensor &_sensorMsg); - - protected: virtual void ProcessModels() = 0; - - protected: virtual void ProcessModel( - const gazebo::msgs::Model &_modelMsg); - - protected: virtual void ProcessModel( - const gazebo::msgs::Model &_modelMsg, VisualPtr _parent); - - protected: virtual VisualPtr Model(const gazebo::msgs::Model &_modelMsg, - VisualPtr _parent); - - protected: virtual void ProcessJoints() = 0; - - protected: virtual void ProcessJoint( - const gazebo::msgs::Joint &_jointMsg); - - protected: virtual void ProcessJoint( - const gazebo::msgs::Joint &_jointMsg, VisualPtr _parent); - - protected: virtual VisualPtr Joint(const gazebo::msgs::Joint &_jointMsg, - VisualPtr _parent); - - protected: virtual void ProcessVisuals() = 0; - - protected: virtual void ProcessVisual( - const gazebo::msgs::Visual &_visualMsg); - - protected: virtual void ProcessVisual( - const gazebo::msgs::Visual &_visualMsg, VisualPtr _parent); - - protected: virtual VisualPtr Visual( - const gazebo::msgs::Visual &_visualMsg, VisualPtr _parent); - - protected: virtual void ProcessLink(const gazebo::msgs::Link &_linkMsg, - VisualPtr _parent); - - protected: virtual VisualPtr Link(const gazebo::msgs::Link &_linkMsg, - VisualPtr _parent); - - protected: virtual VisualPtr Visual(bool _hasId, unsigned int _id, - const std::string &_name, VisualPtr _parent); - - protected: virtual VisualPtr CreateVisual(bool _hasId, unsigned int _id, - const std::string &_name); - - protected: virtual void ProcessGeometry( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessBox( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessCone( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessCapsule( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessCylinder( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessEmpty( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessMesh( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessPlane( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual void ProcessSphere( - const gazebo::msgs::Geometry &_geometryMsg, VisualPtr _parent); - - protected: virtual MaterialPtr CreateMaterial( - const gazebo::msgs::Material &_materialMsg); - - protected: virtual void ProcessPoses() = 0; - - protected: virtual void ProcessPose(const gazebo::msgs::Pose &_poseMsg); - - protected: virtual void SetPose(NodePtr _node, - const gazebo::msgs::Pose &_poseMsg); - - protected: virtual void SetScale(VisualPtr _visual, - const gazebo::msgs::Vector3d &_scaleMsg); - - protected: virtual void ProcessRemovals() = 0; - - protected: virtual void ProcessRemoval(const std::string &_name); - - protected: virtual VisualPtr Parent(const std::string &_name); - - protected: static math::Color Convert( - const gazebo::msgs::Color &_colorMsg); - - protected: static math::Pose3d Convert( - const gazebo::msgs::Pose &_poseMsg); - - protected: static math::Vector3d Convert( - const gazebo::msgs::Vector3d &_vecMsg); - - protected: static math::Quaterniond Convert( - const gazebo::msgs::Quaternion &_quatMsg); - - protected: static ShaderType Convert( - gazebo::msgs::Material::ShaderType _type); - - private: void CreateGeometryFunctionMap(); - - protected: ScenePtr activeScene; - - protected: std::vector scenes; - - protected: std::chrono::steady_clock::duration timePosesReceived; - - protected: std::vector lightMsgs; - - protected: std::vector modelMsgs; - - protected: std::vector jointMsgs; - - protected: std::vector visualMsgs; - - protected: std::vector sensorMsgs; - - protected: std::vector approvedRemovals; - - protected: std::map geomFunctions; - }; - - class CurrentSceneManager : - public virtual SubSceneManager - { - public: CurrentSceneManager(); - - public: virtual ~CurrentSceneManager(); - - public: virtual void OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg); - - protected: virtual void ClearMessages(); - - protected: virtual void ProcessLights(); - - protected: virtual void ProcessModels(); - - protected: virtual void ProcessJoints(); - - protected: virtual void ProcessVisuals(); - - protected: virtual void ProcessSensors(); - - protected: virtual void ProcessPoses(); - - protected: virtual void ProcessRemovals(); - - private: std::map poseMsgs; - }; - - class NewSceneManager : - public virtual SubSceneManager - { - public: NewSceneManager(); - - public: virtual ~NewSceneManager(); - - public: virtual void SetSceneData(const std::string &_data); - - public: virtual void OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg); - - protected: virtual void ProcessMessages(); - - protected: virtual void ClearMessages(); - - protected: virtual void ProcessScene(); - - protected: virtual void ProcessLights(); - - protected: virtual void ProcessModels(); - - protected: virtual void ProcessJoints(); - - protected: virtual void ProcessVisuals(); - - protected: virtual void ProcessSensors(); - - protected: virtual void ProcessPoses(); - - protected: virtual void ProcessPoses( - const gazebo::msgs::PosesStamped &_posesMsg); - - protected: virtual void ProcessRemovals(); - - protected: bool sceneReceived; - - protected: gazebo::msgs::Scene sceneMsg; - - private: std::vector posesMsgs; - }; - } -} -#endif diff --git a/examples/gazebo_scene_viewer/falling_objects.world b/examples/gazebo_scene_viewer/falling_objects.world deleted file mode 100644 index bd62942e1..000000000 --- a/examples/gazebo_scene_viewer/falling_objects.world +++ /dev/null @@ -1,398 +0,0 @@ - - - - - - - - 1.0 - 125 - - - - - 0.5 0.5 0.5 1 - 0 0 0 1 - 1 - - - - - model://sun - - - - model://double_pendulum_with_base - 9 0 2 0 0 0 - - - - 0 0 0 0 0 0 - false - - - 7 0 2 0 0 0 - - - - - - 0.25 - - - - - - - - - - 0.25 - - - - - 0.2 0.0 0.4 1 - 0.3 0.0 0.6 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 0.1 0 0 0 0 - - - - - - 0.5 - - - - - - - - - - 0.5 - - - - - 0.0 0.0 0.3 1 - 0.0 0.0 0.7 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 0 -2 0 0 0 - - - - - - 0.75 - - - - - - - - - - 0.75 - - - - - 0.3 0.0 0.0 1 - 0.7 0.0 0.0 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 2 2 0 0 0 - - - - - - 0.25 - 2 - - - - - - - - - - 0.25 - 2 - - - - - 0 0.4 0 1 - 0 0.6 0 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 2.1 0 0 -0.7 0 - - - - - - 0.5 - 1 - - - - - - - - - - 0.5 - 1 - - - - - 0.1 0.1 0.1 1 - 0.2 0.2 0.2 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 2 -2 0 0 0 - - - - - - 0.75 - 0.5 - - - - - - - - - - 0.75 - 0.5 - - - - - 0.4 0.4 0 1 - 0.6 0.6 0 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 -2 2 0 0 0 - - - - - - 2 1 1 - - - - - - - - - - 2 1 1 - - - - - 0.4 0.2 0 1 - 0.6 0.3 0 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 -2.1 0 0.7 0 0 - - - - - - 1 2 1 - - - - - - - - - - 1 2 1 - - - - - 0 0.4 0.4 1 - 0 0.6 0.6 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - false - - - 7 -2 -2 0 0 0 - - - - - - 1 1 2 - - - - - - - - - - 1 1 2 - - - - - 0.4 0 0.4 1 - 0.6 0 0.6 1 - 0.5 0.5 0.5 1 - - - - - - - - - 0 0 0 0 0 0 - true - - - 0 0 -4 0 0 0 - - - - - - 0 0 1 - 100 100 - - - - - - - - - - 0 0 1 - 100 100 - - - - - 0.25 0.25 0.25 1 - 0.65 0.65 0.65 1 - 0.5 0.5 0.5 1 - - - - - - - - - diff --git a/tutorials.md.in b/tutorials.md.in index 318a3b864..e46d81065 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -15,7 +15,6 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. **The examples** 1. \subpage actor_animation "Actor animation" -2. \subpage gazebo_scene_viewer "Gazebo scene viewer" 3. \subpage mesh_viewer "Mesh viewer" 4. \subpage custom_scene_viewer "Custom scene viewer" 5. \subpage camera_tracking "Camera tracking" diff --git a/tutorials/11_gazebo_scene_viewer_tutorial.md b/tutorials/11_gazebo_scene_viewer_tutorial.md deleted file mode 100644 index 34bab044b..000000000 --- a/tutorials/11_gazebo_scene_viewer_tutorial.md +++ /dev/null @@ -1,72 +0,0 @@ -\page gazebo_scene_viewer Gazebo scene viewer - -The Gazebo scene viewer examples allow us to visualize Gazebo simulation using the Gazebo Rendering library. - -## Compile and run the example - -Clone the source code, create a build directory and use `cmake` and `make` to compile the code: - -```{.sh} -git clone https://github.com/gazebosim/gz-rendering -cd gz-rendering/examples/gazebo_scene_viewer -mkdir build -cd build -cmake .. -make -``` - -### gazebo_scene_viewer - -Launch Gazebo and insert `Double pendulum with base`: - -```{.sh} -gazebo -``` - -Launch the example to visualize the pendulum: - -```{.sh} -./gazebo_scene_viewer -``` - -You can use the `Tab` button to change the rendering engine. - -@image html img/gazebo_scene_viewer.gif - -#### gazebo_scene_viewer2_demo - -Launch Gazebo using the world inside the example directory called `falling_objects.world`. You will see some objects falling. - -```{.sh} -gazebo examples/gazebo_scene_viewer/falling_objects.world -``` - -Launch the example to visualize the objects: - -```{.sh} -./gazebo_scene_viewer2_demo -``` - -You can use the `Tab` button to change the rendering engine. - -![](img/gazebo_scene_viewer2_demo.gif) - -## Code - -The `SceneManager` class defined in `SceneManager.hh`, `SceneManagerPrivate.hh` and `SceneManager.cc` manages a collection of scenes. -The class provides a single interface for modifications, allowing multiple scenes to stay synchronized. -It will allow us to receive data from Gazebo and update the render window each time that the examples receive a new scene. **This class currently consumes Gazebo-specific protobuf messages**. - -The following list will describe some methods: - - - **void SceneManagerPrivate::Init()**: It initializes the communication with Gazebo. It will create some subscribers to receive data about poses, light, models, joints, visual or sensors. - -\snippet examples/gazebo_scene_viewer/SceneManager.cc init scene manager - - - **void SubSceneManager::ProcessMessages()**: This method will process the messages received from Gazebo. Calling the right primitive to render lights, models, joints, visuals, sensors or poses of the different object received. -\snippet examples/gazebo_scene_viewer/SceneManager.cc process message - For example, if the Gazebo scene contains a cylinder the following method will be called: -\snippet examples/gazebo_scene_viewer/SceneManager.cc process cylinder - - - **void CurrentSceneManager::OnPoseUpdate(::ConstPosesStampedPtr &_posesMsg)**: This method is called when the subscriber receives a new pose message. -\snippet examples/gazebo_scene_viewer/SceneManager.cc on update pose