Specify wide angle camera cube map texture format

gazebo/physics/PhysicsEngine_TEST.cc

@@ -126,7 +126,7 @@ void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
     {
       boost::any value;
       double maxStepSize = 0.02;
-      double realTimeUpdateRate = 0.03;
+      double realTimeUpdateRate = 3.03;
       double realTimeFactor = 0.04;
       ignition::math::Vector3d gravity(0, 0, 0);
       ignition::math::Vector3d magField(0.1, 0.1, 0.1);

gazebo/rendering/Camera.cc

@@ -933,9 +933,9 @@ unsigned int Camera::ImageDepth() const
   else if ((imgFmt == "BAYER_RGGB8") || (imgFmt == "BAYER_BGGR8") ||
             (imgFmt == "BAYER_GBRG8") || (imgFmt == "BAYER_GRBG8"))
     return 1;
-  else if (imgFmt == "FLOAT32")
+  else if (imgFmt == "FLOAT32" || imgFmt == "R_FLOAT32")
     return 4;
-  else if (imgFmt == "FLOAT16")
+  else if (imgFmt == "FLOAT16" || imgFmt == "R_FLOAT16")
     return 2;
   else
   {
@@ -1004,9 +1004,9 @@ int Camera::OgrePixelFormat(const std::string &_format)
     result = static_cast<int>(Ogre::PF_BYTE_RGB);
   else if (_format == "B8G8R8" || _format == "BGR_INT8")
     result = static_cast<int>(Ogre::PF_BYTE_BGR);
-  else if (_format == "FLOAT32")
+  else if (_format == "FLOAT32" || _format == "R_FLOAT32")
     result = static_cast<int>(Ogre::PF_FLOAT32_R);
-  else if (_format == "FLOAT16")
+  else if (_format == "FLOAT16" || _format == "R_FLOAT16")
     result = static_cast<int>(Ogre::PF_FLOAT16_R);
   else if (_format == "R16G16B16" || _format == "RGB_INT16"
       || _format == "RGB_UINT16")

gazebo/rendering/Camera.hh

@@ -869,6 +869,11 @@ namespace gazebo
       /// \return Integer representation of the Ogre image format
       private: static int OgrePixelFormat(const std::string &_format);
 
+      /// \brief Allow WideAngleCamera::Load to call OgrePixelFormat.
+      /// To avoid needing to include WideAngleCamera.hh, just befriend
+      /// the entire class.
+      friend class WideAngleCamera;
+
       /// \brief Receive command message.
       /// \param[in] _msg Camera Command message.
       private: void OnCmdMsg(ConstCameraCmdPtr &_msg);

gazebo/rendering/Camera_TEST.cc

@@ -191,7 +191,7 @@ TEST_F(Camera_TEST, Create)
 
     unsigned int width = 500;
     unsigned int height = 300;
-    std::string format = "FLOAT16";
+    std::string format = "R_FLOAT16";
     double hfov = 1.05;
     double near = 0.001;
     double far = 200.0;
@@ -239,7 +239,7 @@ TEST_F(Camera_TEST, Create)
 
     unsigned int width = 500;
     unsigned int height = 300;
-    std::string format = "FLOAT32";
+    std::string format = "R_FLOAT32";
     double hfov = 1.05;
     double near = 0.001;
     double far = 200.0;

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,

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;
 

test/integration/wideanglecamera_sensor.cc

@@ -225,3 +225,112 @@ TEST_F(WideAngleCameraSensor, Projection)
   EXPECT_LT(screenPt.Z(), 1.0);
 #endif
 }
+
+/////////////////////////////////////////////////
+TEST_F(WideAngleCameraSensor, TextureFormat)
+{
+#if not defined(__APPLE__)
+  Load("worlds/test/issue_2928_wide_angle_camera_texture_format.world");
+
+  // Make sure the render engine is available.
+  if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
+      rendering::RenderEngine::NONE)
+  {
+    gzerr << "No rendering engine, unable to run wide angle camera test\n";
+    return;
+  }
+
+  const unsigned int width  = 12;
+  const unsigned int height = 12;
+
+  {  // check camera with default texture format
+    sensors::SensorPtr sensor = sensors::get_sensor(
+      "wide_angle_camera_sensor_with_default_texture");
+    sensors::WideAngleCameraSensorPtr cameraSensor =
+        std::dynamic_pointer_cast<sensors::WideAngleCameraSensor>(sensor);
+
+    imageCount = 0;
+    img = new unsigned char[width * height * 3];
+    event::ConnectionPtr c =
+      cameraSensor->Camera()->ConnectNewImageFrame(
+          std::bind(&::OnNewCameraFrame, &imageCount, img,
+            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+            std::placeholders::_4, std::placeholders::_5));
+
+    // Get some images
+    int sleep = 0;
+    int maxSleep = 30;
+    while (imageCount < 10 && sleep < maxSleep)
+    {
+      common::Time::MSleep(50);
+      sleep++;
+    }
+
+    unsigned int rSum = 0;
+    unsigned int gSum = 0;
+    unsigned int bSum = 0;
+    for (unsigned int i = 0; i < height*width*3; i+=3)
+    {
+      unsigned int r = img[i];
+      unsigned int g = img[i+1];
+      unsigned int b = img[i+2];
+      rSum += r;
+      gSum += g;
+      bSum += b;
+    }
+
+    EXPECT_NE(rSum, gSum);
+    EXPECT_NE(rSum, bSum);
+    EXPECT_NE(gSum, bSum);
+
+    EXPECT_GT(rSum - bSum, 30000.0);
+    EXPECT_GT(rSum - gSum, 20000.0);
+
+    delete [] img;
+  }
+
+  {  // check camera with grayscale texture format
+    sensors::SensorPtr sensor = sensors::get_sensor(
+      "wide_angle_camera_sensor_with_grayscale_texture");
+    sensors::WideAngleCameraSensorPtr cameraSensor =
+        std::dynamic_pointer_cast<sensors::WideAngleCameraSensor>(sensor);
+
+    imageCount = 0;
+    img = new unsigned char[width * height * 3];
+    event::ConnectionPtr c =
+      cameraSensor->Camera()->ConnectNewImageFrame(
+          std::bind(&::OnNewCameraFrame, &imageCount, img,
+            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+            std::placeholders::_4, std::placeholders::_5));
+
+    // Get some images
+    int sleep = 0;
+    int maxSleep = 30;
+    while (imageCount < 10 && sleep < maxSleep)
+    {
+      common::Time::MSleep(50);
+      sleep++;
+    }
+
+    unsigned int rSum = 0;
+    unsigned int gSum = 0;
+    unsigned int bSum = 0;
+    for (unsigned int i = 0; i < height*width*3; i+=3)
+    {
+      unsigned int r = img[i];
+      unsigned int g = img[i+1];
+      unsigned int b = img[i+2];
+      rSum += r;
+      gSum += g;
+      bSum += b;
+    }
+
+    // For grayscale, all RGB channels should have the same value
+    EXPECT_EQ(rSum, gSum);
+    EXPECT_EQ(gSum, bSum);
+    EXPECT_GT(gSum, 10000u);
+
+    delete [] img;
+  }
+#endif
+}

worlds/test/issue_2928_wide_angle_camera_texture_format.world

@@ -0,0 +1,140 @@
+<?xml version="1.0" ?>
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 0 0 -0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-1 0 -0.2</direction>
+      <attenuation>
+        <range>10</range>
+      </attenuation>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <scene>
+      <shadows>0</shadows>
+    </scene>
+    <model name='wide_angle_camera_with_default_texture'>
+      <static>1</static>
+      <pose>0.3 0 0.15 0 0 3.14159265</pose>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name='wide_angle_camera_sensor_with_default_texture' type='wideanglecamera'>
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>12</width>
+              <height>12</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>gnomonical</type>
+              <scale_to_hfov>1</scale_to_hfov>
+              <cutoff_angle>1.5707</cutoff_angle>
+              <env_texture_size>512</env_texture_size>
+            </lens>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>1</visualize>
+        </sensor>
+      </link>
+    </model>
+    <model name='wide_angle_camera_with_grayscale_texture'>
+      <static>1</static>
+      <pose>0.3 2 0.15 0 0 3.14159265</pose>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name='wide_angle_camera_sensor_with_grayscale_texture' type='wideanglecamera'>
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>12</width>
+              <height>12</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>gnomonical</type>
+              <scale_to_hfov>1</scale_to_hfov>
+              <cutoff_angle>1.5707</cutoff_angle>
+              <env_texture_size>512</env_texture_size>
+              <gazebo:env_texture_format>R_FLOAT16</gazebo:env_texture_format>
+            </lens>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>1</visualize>
+        </sensor>
+      </link>
+    </model>
+    <model name='Construction Cone 1'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+      </link>
+      <pose>0 0 0 0 0 0</pose>
+    </model>
+    <model name='Construction Cone 2'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+      </link>
+      <pose>0 2 0 0 0 0</pose>
+    </model>
+  </world>
+</sdf>