From d3793e344891d6a3224ab9b42b6ad8b934a217da Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Wed, 3 Feb 2021 18:00:12 -0800
Subject: [PATCH] WideAngleCamera: specify cube map texture format

The WideAngleCamera has a texture for a cube map that
uses the same pixel format as the camera image. If there
are intermediate shaders applied to the camera, the choice
of this image format may cause pixel values to be truncated.
To provide flexibility to the user, the pixel format for
the cube map texture can be specified with a custom SDFormat
element in `//camera/lens/gazebo:env_texture_format`
using the same values as `//camera/image/format`.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 gazebo/rendering/WideAngleCamera.cc        | 13 ++++++++++++-
 gazebo/rendering/WideAngleCameraPrivate.hh |  4 ++++
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/gazebo/rendering/WideAngleCamera.cc b/gazebo/rendering/WideAngleCamera.cc
index ffd517d799..16f6a66b64 100644
--- a/gazebo/rendering/WideAngleCamera.cc
+++ b/gazebo/rendering/WideAngleCamera.cc
@@ -457,6 +457,10 @@ void WideAngleCamera::Load()
 {
   Camera::Load();
 
+  // Cube map texture format defaults to matching image pixel format
+  this->dataPtr->envCubeMapTextureFormat =
+    static_cast<Ogre::PixelFormat>(this->imageFormat);
+
   this->CreateEnvCameras();
 
   if (this->sdf->HasElement("lens"))
@@ -467,6 +471,13 @@ void WideAngleCamera::Load()
 
     if (sdfLens->HasElement("env_texture_size"))
       this->dataPtr->envTextureSize = sdfLens->Get<int>("env_texture_size");
+
+    const std::string envTextureFormat = "gazebo:env_texture_format";
+    if (sdfLens->HasElement(envTextureFormat))
+    {
+      this->dataPtr->envCubeMapTextureFormat = static_cast<Ogre::PixelFormat>(
+        this->OgrePixelFormat(sdfLens->Get<std::string>(envTextureFormat)));
+    }
   }
   else
     this->dataPtr->lens->Load();
@@ -660,7 +671,7 @@ void WideAngleCamera::CreateEnvRenderTexture(const std::string &_textureName)
           this->dataPtr->envTextureSize,
           this->dataPtr->envTextureSize,
           0,
-          static_cast<Ogre::PixelFormat>(this->imageFormat),
+          this->dataPtr->envCubeMapTextureFormat,
           Ogre::TU_RENDERTARGET,
           0,
           false,
diff --git a/gazebo/rendering/WideAngleCameraPrivate.hh b/gazebo/rendering/WideAngleCameraPrivate.hh
index cbf21c414f..dcda17c843 100644
--- a/gazebo/rendering/WideAngleCameraPrivate.hh
+++ b/gazebo/rendering/WideAngleCameraPrivate.hh
@@ -20,6 +20,7 @@
 #include <mutex>
 
 #include "gazebo/msgs/msgs.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
 #include "gazebo/util/system.hh"
 
 
@@ -59,6 +60,9 @@ namespace gazebo
       /// \brief Viewports for the render targets
       public: Ogre::Viewport *envViewports[6];
 
+      /// \brief Pixel format for cube map texture
+      public: Ogre::PixelFormat envCubeMapTextureFormat;
+
       /// \brief A single cube map texture
       public: Ogre::Texture *envCubeMapTexture;