From 60e823efa97bafffe958f7490bb4e84941c645f3 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 26 Jan 2021 19:41:50 +0100 Subject: [PATCH] add_laser_retro (and fix DCO) Signed-off-by: Guillaume Doisy --- .../ignition/rendering/ogre2/Ogre2GpuRays.hh | 13 + ogre2/src/Ogre2GpuRays.cc | 249 +++++++++++++++++- .../programs/gpu_rays_1st_pass_fs.glsl | 6 +- .../programs/gpu_rays_2nd_pass_fs.glsl | 10 +- .../media/materials/scripts/gpu_rays.material | 48 ++++ test/integration/gpu_rays.cc | 15 ++ 6 files changed, 323 insertions(+), 18 deletions(-) diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2GpuRays.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2GpuRays.hh index 9bc272ad2..58506ed56 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2GpuRays.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2GpuRays.hh @@ -23,11 +23,24 @@ #include "ignition/rendering/RenderTypes.hh" #include "ignition/rendering/base/BaseGpuRays.hh" +#include "ignition/rendering/ogre2/Export.hh" +#include "ignition/rendering/ogre2/Ogre2Includes.hh" #include "ignition/rendering/ogre2/Ogre2RenderTarget.hh" #include "ignition/rendering/ogre2/Ogre2RenderTypes.hh" #include "ignition/rendering/ogre2/Ogre2Scene.hh" #include "ignition/rendering/ogre2/Ogre2Sensor.hh" +#include "ignition/common/Event.hh" +#include "ignition/common/Console.hh" + +namespace Ogre +{ + class Material; + class RenderTarget; + class Texture; + class Viewport; +} + namespace ignition { namespace rendering diff --git a/ogre2/src/Ogre2GpuRays.cc b/ogre2/src/Ogre2GpuRays.cc index 7f27686cb..ac52ba2d8 100644 --- a/ogre2/src/Ogre2GpuRays.cc +++ b/ogre2/src/Ogre2GpuRays.cc @@ -18,9 +18,68 @@ #include #include +#include +#include + #include "ignition/rendering/ogre2/Ogre2Camera.hh" #include "ignition/rendering/ogre2/Ogre2GpuRays.hh" #include "ignition/rendering/ogre2/Ogre2RenderEngine.hh" +#include "ignition/rendering/RenderTypes.hh" +#include "ignition/rendering/ogre2/Ogre2Conversions.hh" +#include "ignition/rendering/ogre2/Ogre2Includes.hh" +#include "ignition/rendering/ogre2/Ogre2RenderTarget.hh" +#include "ignition/rendering/ogre2/Ogre2RenderTypes.hh" +#include "ignition/rendering/ogre2/Ogre2Scene.hh" +#include "ignition/rendering/ogre2/Ogre2Sensor.hh" +#include "ignition/rendering/ogre2/Ogre2Visual.hh" + +namespace ignition +{ +namespace rendering +{ +inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { +// +/// \brief Helper class for switching the ogre item's material to laser retro +/// source material when a thermal camera is being rendered. +class Ogre2LaserRetroMaterialSwitcher : public Ogre::RenderTargetListener +{ + /// \brief constructor + /// \param[in] _scene the scene manager responsible for rendering + public: explicit Ogre2LaserRetroMaterialSwitcher(Ogre2ScenePtr _scene); + + /// \brief destructor + public: ~Ogre2LaserRetroMaterialSwitcher() = default; + + /// \brief Callback when a render target is about to be rendered + /// \param[in] _evt Ogre render target event containing information about + /// the source render target. + private: virtual void preRenderTargetUpdate( + const Ogre::RenderTargetEvent &_evt) override; + + /// \brief Callback when a render target is finisned being rendered + /// \param[in] _evt Ogre render target event containing information about + /// the source render target. + private: virtual void postRenderTargetUpdate( + const Ogre::RenderTargetEvent &_evt) override; + + /// \brief Scene manager + private: Ogre2ScenePtr scene = nullptr; + + /// \brief Pointer to the laser retro source material + private: Ogre::MaterialPtr laserRetroSourceMaterial; + + /// \brief Custom parameter index of laser retro value in an ogre subitem. + /// This has to match the custom index specifed in LaserRetroSource material + /// script in media/materials/scripts/gpu_rays.material + private: const unsigned int customParamIdx = 10u; + + /// \brief A map of ogre sub item pointer to their original hlms material + private: std::map datablockMap; +}; +} +} +} + /// \internal /// \brief Private data for the Ogre2GpuRays class @@ -99,11 +158,132 @@ class ignition::rendering::Ogre2GpuRaysPrivate /// \brief Dummy render texture for the gpu rays public: RenderTexturePtr renderTexture; + + /// \brief Pointer to material switcher + public: std::unique_ptr + laserRetroMaterialSwitcher[6]; }; using namespace ignition; using namespace rendering; + +////////////////////////////////////////////////// +Ogre2LaserRetroMaterialSwitcher::Ogre2LaserRetroMaterialSwitcher( + Ogre2ScenePtr _scene) +{ + this->scene = _scene; + // plain opaque material + Ogre::ResourcePtr res = + Ogre::MaterialManager::getSingleton().load("LaserRetroSource", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + this->laserRetroSourceMaterial = res.staticCast(); + this->laserRetroSourceMaterial->load(); +} + +////////////////////////////////////////////////// +void Ogre2LaserRetroMaterialSwitcher::preRenderTargetUpdate( + const Ogre::RenderTargetEvent & /*_evt*/) +{ + // swap item to use v1 shader material + // Note: keep an eye out for performance impact on switching materials + // on the fly. We are not doing this often so should be ok. + this->datablockMap.clear(); + auto itor = this->scene->OgreSceneManager()->getMovableObjectIterator( + Ogre::ItemFactory::FACTORY_TYPE_NAME); + while (itor.hasMoreElements()) + { + Ogre::MovableObject *object = itor.peekNext(); + Ogre::Item *item = static_cast(object); + + std::string laserRetroKey = "laser_retro"; + // get visual + Ogre::Any userAny = item->getUserObjectBindings().getUserAny(); + if (!userAny.isEmpty() && userAny.getType() == typeid(unsigned int)) + { + VisualPtr result; + try + { + result = this->scene->VisualById(Ogre::any_cast(userAny)); + } + catch(Ogre::Exception &e) + { + ignerr << "Ogre Error:" << e.getFullDescription() << "\n"; + } + Ogre2VisualPtr ogreVisual = + std::dynamic_pointer_cast(result); + + // get laser_retro + Variant tempLaserRetro = ogreVisual->UserData(laserRetroKey); + + float retroValue = -1.0; + try + { + retroValue = std::get(tempLaserRetro); + } + catch(...) + { + try + { + retroValue = std::get(tempLaserRetro); + } + catch(...) + { + try + { + retroValue = std::get(tempLaserRetro); + } + catch(std::bad_variant_access &e) + { + ignerr << "Error casting user data: " << e.what() << "\n"; + retroValue = -1.0; + } + } + } + + // only accept positive laser retro value + if (retroValue >= 0) + { + // set visibility flag so the camera can see it + item->addVisibilityFlags(0x01000000); + for (unsigned int i = 0; i < item->getNumSubItems(); ++i) + { + Ogre::SubItem *subItem = item->getSubItem(i); + if (!subItem->hasCustomParameter(this->customParamIdx)) + { + // limit laser retro value to 2000 (as in gazebo) + if (retroValue > 2000.0){ + retroValue = 2000.0; + } + float color = retroValue / 2000.0; + subItem->setCustomParameter(this->customParamIdx, + Ogre::Vector4(color, color, color, 1.0)); + } + Ogre::HlmsDatablock *datablock = subItem->getDatablock(); + this->datablockMap[subItem] = datablock; + + subItem->setMaterial(this->laserRetroSourceMaterial); + } + } + } + itor.moveNext(); + } + } + +////////////////////////////////////////////////// +void Ogre2LaserRetroMaterialSwitcher::postRenderTargetUpdate( + const Ogre::RenderTargetEvent & /*_evt*/) +{ + // restore item to use hlms material + for (auto it : this->datablockMap) + { + Ogre::SubItem *subItem = it.first; + subItem->setDatablock(it.second); + } +} + + ////////////////////////////////////////////////// Ogre2GpuRays::Ogre2GpuRays() : dataPtr(new Ogre2GpuRaysPrivate) @@ -115,6 +295,7 @@ Ogre2GpuRays::Ogre2GpuRays() { this->dataPtr->cubeCam[i] = nullptr; this->dataPtr->ogreCompositorWorkspace1st[i] = nullptr; + this->dataPtr->laserRetroMaterialSwitcher[i] = nullptr; } } @@ -439,7 +620,8 @@ void Ogre2GpuRays::Setup1stPass() // { // in 0 rt_input // texture depthTexture target_width target_height PF_D32_FLOAT - // target depthTexture + // texture colorTexture target_width target_height PF_R8G8B8 + // target colorTexture // { // pass clear // { @@ -459,6 +641,7 @@ void Ogre2GpuRays::Setup1stPass() // { // material GpuRaysScan1st // Use copy instead of original // input 0 depthTexture + // input 1 colorTexture // quad_normals camera_far_corners_view_space // } // } @@ -489,28 +672,49 @@ void Ogre2GpuRays::Setup1stPass() depthTexDef->uav = false; depthTexDef->automipmaps = false; depthTexDef->hwGammaWrite = Ogre::TextureDefinitionBase::BoolFalse; - depthTexDef->depthBufferId = Ogre::DepthBuffer::POOL_NON_SHAREABLE; + depthTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + depthTexDef->depthBufferFormat = Ogre::PF_UNKNOWN; depthTexDef->fsaaExplicitResolve = false; - nodeDef->setNumTargetPass(2); - // depthTexture target - renders depth - Ogre::CompositorTargetDef *depthTargetDef = - nodeDef->addTargetPass("depthTexture"); - depthTargetDef->setNumPasses(2); + Ogre::TextureDefinitionBase::TextureDefinition *colorTexDef = + nodeDef->addTextureDefinition("colorTexture"); + colorTexDef->textureType = Ogre::TEX_TYPE_2D; + colorTexDef->width = 0; + colorTexDef->height = 0; + colorTexDef->depth = 1; + colorTexDef->numMipmaps = 0; + colorTexDef->widthFactor = 1; + colorTexDef->heightFactor = 1; + colorTexDef->formatList = {Ogre::PF_R8G8B8}; + colorTexDef->fsaa = 0; + colorTexDef->uav = false; + colorTexDef->automipmaps = false; + colorTexDef->hwGammaWrite = Ogre::TextureDefinitionBase::BoolFalse; + colorTexDef->depthBufferId = Ogre::DepthBuffer::POOL_DEFAULT; + colorTexDef->depthBufferFormat = Ogre::PF_D32_FLOAT; + colorTexDef->preferDepthTexture = true; + colorTexDef->fsaaExplicitResolve = false; + + nodeDef->setNumTargetPass(2); + + Ogre::CompositorTargetDef *colorTargetDef = + nodeDef->addTargetPass("colorTexture"); + colorTargetDef->setNumPasses(2); { // clear pass Ogre::CompositorPassClearDef *passClear = static_cast( - depthTargetDef->addPass(Ogre::PASS_CLEAR)); - passClear->mColourValue = Ogre::ColourValue(this->dataMaxVal, 0, 1.0); + colorTargetDef->addPass(Ogre::PASS_CLEAR)); + passClear->mColourValue = Ogre::ColourValue(0, 0, 0); // scene pass Ogre::CompositorPassSceneDef *passScene = static_cast( - depthTargetDef->addPass(Ogre::PASS_SCENE)); - passScene->mVisibilityMask = IGN_VISIBILITY_ALL - & ~(IGN_VISIBILITY_GUI | IGN_VISIBILITY_SELECTABLE); + colorTargetDef->addPass(Ogre::PASS_SCENE)); + // set camera custom visibility mask when rendering laser retro + passScene->mVisibilityMask = 0x01000000; } + // rt_input target - converts depth to range Ogre::CompositorTargetDef *inputTargetDef = nodeDef->addTargetPass("rt_input"); @@ -527,6 +731,7 @@ void Ogre2GpuRays::Setup1stPass() inputTargetDef->addPass(Ogre::PASS_QUAD)); passQuad->mMaterialName = this->dataPtr->matFirstPass->getName(); passQuad->addQuadTextureSource(0, "depthTexture", 0); + passQuad->addQuadTextureSource(1, "colorTexture", 0); passQuad->mFrustumCorners = Ogre::CompositorPassQuadDef::VIEW_SPACE_CORNERS; } @@ -580,7 +785,7 @@ void Ogre2GpuRays::Setup1stPass() Ogre::TextureManager::getSingleton().createManual( texName.str(), "General", Ogre::TEX_TYPE_2D, this->dataPtr->w1st, this->dataPtr->h1st, 1, 0, - Ogre::PF_FLOAT32_R, Ogre::TU_RENDERTARGET, + Ogre::PF_FLOAT32_RGB, Ogre::TU_RENDERTARGET, 0, false, 0, Ogre::BLANKSTRING, false, true); Ogre::RenderTarget *rt = @@ -589,6 +794,24 @@ void Ogre2GpuRays::Setup1stPass() this->dataPtr->ogreCompositorWorkspace1st[i] = ogreCompMgr->addWorkspace(this->scene->OgreSceneManager(), rt, this->dataPtr->cubeCam[i], wsDefName, false); + + // add laser retro material switcher to render target listener + // so we can switch to use laser retro material when the camera is being udpated + Ogre::CompositorNode *node = + this->dataPtr->ogreCompositorWorkspace1st[i]->getNodeSequence()[0]; + auto channelsTex = node->getLocalTextures(); + + for (auto c : channelsTex) + { + if (c.textures[0]->getSrcFormat() == Ogre::PF_R8G8B8) + { + this->dataPtr->laserRetroMaterialSwitcher[i].reset( + new Ogre2LaserRetroMaterialSwitcher(this->scene)); + c.target->addListener(this->dataPtr->laserRetroMaterialSwitcher[i].get()); + break; + } + } + } } diff --git a/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl b/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl index 3ee4727bc..d01d153ac 100644 --- a/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl +++ b/ogre2/src/media/materials/programs/gpu_rays_1st_pass_fs.glsl @@ -24,6 +24,7 @@ in block } inPs; uniform sampler2D depthTexture; +uniform sampler2D colorTexture; out vec4 fragColor; @@ -45,6 +46,9 @@ void main() { // get linear depth float d = getDepth(inPs.uv0); + + // get retro + float retro = texture(colorTexture, inPs.uv0).x * 2000.0; // reconstruct 3d viewspace pos from depth vec3 viewSpacePos = inPs.cameraDir * d; @@ -57,5 +61,5 @@ void main() else if (l < near) l = min; - fragColor = vec4(l, 0.0, 0, 1.0); + fragColor = vec4(l, retro, 0.0, 1.0); } diff --git a/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl b/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl index 7f7813e6c..dab691092 100644 --- a/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl +++ b/ogre2/src/media/materials/programs/gpu_rays_2nd_pass_fs.glsl @@ -54,9 +54,9 @@ uniform sampler2D tex5; out vec4 fragColor; -float getRange(vec2 uv, sampler2D tex) +vec2 getRange(vec2 uv, sampler2D tex) { - float range = texture(tex, uv).x; + vec2 range = texture(tex, uv).xy; return range; } @@ -71,7 +71,9 @@ void main() // uv coordinates on texture that stores the range data vec2 uv = data.xy; - float d = 0; + vec2 d; + d.x = 0; + d.y = 0; if (faceIdx == 0) d = getRange(uv, tex0); else if (faceIdx == 1) @@ -88,6 +90,6 @@ void main() // todo(anyone) set retro values float retro = 0.0; - fragColor = vec4(d, retro, 0, 1.0); + fragColor = vec4(d.x, d.y, 0, 1.0); return; } diff --git a/ogre2/src/media/materials/scripts/gpu_rays.material b/ogre2/src/media/materials/scripts/gpu_rays.material index 5eeb06685..5730b8ddd 100644 --- a/ogre2/src/media/materials/scripts/gpu_rays.material +++ b/ogre2/src/media/materials/scripts/gpu_rays.material @@ -31,6 +31,7 @@ fragment_program GpuRaysScan1stFS glsl default_params { param_named depthTexture int 0 + param_named colorTexture int 1 } } @@ -47,6 +48,11 @@ material GpuRaysScan1st filtering none tex_address_mode clamp } + texture_unit colorTexture + { + filtering none + tex_address_mode clamp + } } } } @@ -106,3 +112,45 @@ material GpuRaysScan2nd } } } + +vertex_program laser_retro_vs glsl +{ + source plain_color_vs.glsl + + default_params + { + param_named_auto worldViewProj worldviewproj_matrix + } +} + +fragment_program laser_retro_fs glsl +{ + source plain_color_fs.glsl + + default_params + { + param_named inColor float4 0 0 0 1 + } +} + +material LaserRetroSource +{ + // Material has one technique + technique + { + // This technique has one pass + pass + { + fog_override true + + vertex_program_ref laser_retro_vs + { + } + + fragment_program_ref laser_retro_fs + { + param_named_auto inColor custom 10 + } + } + } +} diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index 5948da0b5..79e723d19 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -217,6 +217,11 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine) gpuRays2->SetVerticalRayCount(vRayCount); root->AddChild(gpuRays2); + // Laser retro test values + double laserRetro1 = 2000; + double laserRetro2 = 1000; + std::string userDataKey = "laser_retro"; + // Create testing boxes // box in the center ignition::math::Pose3d box01Pose(ignition::math::Vector3d(3, 0, 0.5), @@ -225,8 +230,11 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine) visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetWorldPosition(box01Pose.Pos()); visualBox1->SetWorldRotation(box01Pose.Rot()); + visualBox1->SetUserData(userDataKey, laserRetro1); root->AddChild(visualBox1); + + // box on the right of the first gpu rays caster ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5), ignition::math::Quaterniond::Identity); @@ -234,6 +242,7 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine) visualBox2->AddGeometry(scene->CreateBox()); visualBox2->SetWorldPosition(box02Pose.Pos()); visualBox2->SetWorldRotation(box02Pose.Rot()); + visualBox2->SetUserData(userDataKey, laserRetro2); root->AddChild(visualBox2); // box on the left of the rays caster 1 but out of range @@ -269,6 +278,12 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine) EXPECT_NEAR(scan[0], expectedRangeAtMidPointBox2, LASER_TOL); EXPECT_DOUBLE_EQ(scan[last], ignition::math::INF_D); + // rays cater should see box01 with laser retro value set to laserRetro1 + // and box02 with laser retro value set to laserRetro2 + EXPECT_NEAR(scan[mid+1], laserRetro1, 5.0); + EXPECT_NEAR(scan[0+1], laserRetro2, 5.0); + EXPECT_DOUBLE_EQ(scan[last+1], 0.0); + // Verify rays caster 2 range readings // listen to new gpu rays frames float *scan2 = new float[hRayCount * vRayCount * 3];