Fix wide-angle lens flare occlusion lag

gazebo/rendering/LensFlare.cc

@@ -305,6 +305,8 @@ namespace gazebo
                 ignition::math::Pose3d(
                   Conversions::ConvertIgn(cam->getDerivedPosition()),
                   Conversions::ConvertIgn(quat)));
+            this->dataPtr->wideAngleDummyCamera->OgreCamera()
+                ->getParentSceneNode()->_update(true, true);
 
             // OcclusionScale() was built for a regular perspective projection
             // camera and cannot be passed a WideAngleCamera. A cleaner solution

test/integration/camera_sensor.cc

@@ -1433,8 +1433,11 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
   physics::ModelPtr model = world->ModelByName(modelNameLensFlare);
   ASSERT_TRUE(model != nullptr);
 
+  // use fully scoped sensor name
   sensors::SensorPtr sensorLensFlare =
-    sensors::get_sensor(cameraNameLensFlare);
+    sensors::get_sensor(world->Name() + "::"
+                 + modelNameLensFlare + "::link::"
+                 + cameraNameLensFlare);
   sensors::CameraSensorPtr camSensorLensFlare =
     std::dynamic_pointer_cast<sensors::CameraSensor>(sensorLensFlare);
   ASSERT_TRUE(camSensorLensFlare != nullptr);
@@ -1446,8 +1449,11 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
   model = world->ModelByName(modelNameWithoutLensFlare);
   ASSERT_TRUE(model != nullptr);
 
+  // use fully scoped sensor name
   sensors::SensorPtr sensorWithoutLensFlare =
-    sensors::get_sensor(cameraNameWithoutLensFlare);
+    sensors::get_sensor(world->Name() + "::"
+          + modelNameWithoutLensFlare + "::link::"
+          + cameraNameWithoutLensFlare);
   sensors::CameraSensorPtr camSensorWithoutLensFlare =
     std::dynamic_pointer_cast<sensors::CameraSensor>(sensorWithoutLensFlare);
   ASSERT_TRUE(camSensorWithoutLensFlare != nullptr);
@@ -1512,6 +1518,166 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
   delete[] img2;
 }
 
+/////////////////////////////////////////////////
+TEST_F(CameraSensor, LensFlareWideAngleCameraOcclusion)
+{
+  LoadArgs(" --lockstep -u worlds/lensflare_wideangle_cam.world");
+
+  // Make sure the render engine is available.
+  if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
+      rendering::RenderEngine::NONE)
+  {
+    gzerr << "No rendering engine, unable to run camera test\n";
+    return;
+  }
+
+  // Get the model containing the wide angle cameras.
+  const std::string modelName = "slow_wide_angle_cameras";
+  const std::string cameraNameLensFlare = "camera_sensor_lensflare";
+
+  physics::WorldPtr world = physics::get_world();
+  ASSERT_TRUE(world != nullptr);
+  physics::ModelPtr model = world->ModelByName(modelName);
+  ASSERT_TRUE(model != nullptr);
+
+  // use fully scoped sensor name
+  sensors::SensorPtr sensorLensFlare =
+    sensors::get_sensor(world->Name() + "::"
+                          + modelName + "::link::" + cameraNameLensFlare);
+  sensors::CameraSensorPtr camSensorLensFlare =
+    std::dynamic_pointer_cast<sensors::CameraSensor>(sensorLensFlare);
+  ASSERT_TRUE(camSensorLensFlare != nullptr);
+
+  // Get the wide angle camera without the lens flare.
+  std::string cameraNameWithoutLensFlare = "camera_sensor_without_lensflare";
+
+  // use fully scoped sensor name
+  sensors::SensorPtr sensorWithoutLensFlare =
+    sensors::get_sensor(world->Name() + "::"
+                          + modelName + "::link::" + cameraNameWithoutLensFlare);
+  sensors::CameraSensorPtr camSensorWithoutLensFlare =
+    std::dynamic_pointer_cast<sensors::CameraSensor>(sensorWithoutLensFlare);
+  ASSERT_TRUE(camSensorWithoutLensFlare != nullptr);
+
+  const unsigned int width = 320;
+  const unsigned int height = 240;
+
+  // Collect images from the 2 cameras.
+  imageCount = 0;
+  imageCount2 = 0;
+  img = new unsigned char[width * height * 3];
+  img2 = new unsigned char[width * height * 3];
+  event::ConnectionPtr c =
+    camSensorLensFlare->Camera()->ConnectNewImageFrame(
+        std::bind(&::OnNewCameraFrame, &imageCount, img,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+          std::placeholders::_4, std::placeholders::_5));
+  event::ConnectionPtr c2 =
+    camSensorWithoutLensFlare->Camera()->ConnectNewImageFrame(
+        std::bind(&::OnNewCameraFrame, &imageCount2, img2,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+          std::placeholders::_4, std::placeholders::_5));
+
+  // Step just enough to trigger new images, then wait for images to arrive.
+  for (int i = 0; i < 3; ++i)
+  {
+    // Sensor update period of 1 second
+    // Step 1.005 seconds to ensure images are triggered
+    world->Step(1005);
+
+    int sleep = 0;
+    while ((imageCount < i+1 || imageCount2 < i+1)
+        && sleep++ < 1000)
+    {
+      common::Time::MSleep(10);
+    }
+    gzdbg << "Sim time " << world->SimTime().Double()
+          << ", slept for " << sleep * 0.01 << " seconds"
+          << std::endl;
+
+    EXPECT_EQ(imageCount, i + 1);
+    EXPECT_EQ(imageCount2, i + 1);
+  }
+
+  EXPECT_EQ(imageCount, 3);
+  EXPECT_EQ(imageCount2, 3);
+
+  // Lock image mutex to allow safe comparison of images
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    // The lens flare should be occluded at the initial position, so expect
+    // the images with and without lens flare to be identical.
+    for (unsigned int y = 0; y < height; ++y)
+    {
+      for (unsigned int x = 0; x < width*3; x+=3)
+      {
+        EXPECT_NEAR(img[(y*width*3) + x], img2[(y*width*3) + x], 2);
+        EXPECT_NEAR(img[(y*width*3) + x + 1], img2[(y*width*3) + x + 1], 2);
+        EXPECT_NEAR(img[(y*width*3) + x + 2], img2[(y*width*3) + x + 2], 2);
+      }
+    }
+  }
+
+  // Move the model up 10 meters so that it is no longer occluded.
+  auto modelPose = model->WorldPose();
+  modelPose.SetZ(modelPose.Pos().Z() + 10);
+  model->SetWorldPose(modelPose);
+
+  // Step an additional 1.005 seconds.
+  // This should generate another pair of images.
+  world->Step(1005);
+  {
+    int sleep = 0;
+    while ((imageCount < 4 || imageCount2 < 4)
+        && sleep++ < 1000)
+    {
+      common::Time::MSleep(10);
+    }
+    gzdbg << "Sim time " << world->SimTime().Double()
+          << ", slept for " << sleep * 0.01 << " seconds"
+          << std::endl;
+  }
+  EXPECT_EQ(imageCount, 4);
+  EXPECT_EQ(imageCount2, 4);
+
+  c.reset();
+  c2.reset();
+
+  unsigned int colorSum = 0;
+  unsigned int colorSum2 = 0;
+  // Lock image mutex to allow safe comparison of images
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    EXPECT_EQ(imageCount, 4);
+    EXPECT_EQ(imageCount2, 4);
+
+    for (unsigned int y = 0; y < height; ++y)
+    {
+      for (unsigned int x = 0; x < width*3; x+=3)
+      {
+        unsigned int r = img[(y*width*3) + x];
+        unsigned int g = img[(y*width*3) + x + 1];
+        unsigned int b = img[(y*width*3) + x + 2];
+        colorSum += r + g + b;
+        unsigned int r2 = img2[(y*width*3) + x];
+        unsigned int g2 = img2[(y*width*3) + x + 1];
+        unsigned int b2 = img2[(y*width*3) + x + 2];
+        colorSum2 += r2 + g2 + b2;
+      }
+    }
+  }
+
+  // Camera with lens flare should be brighter than camera without it.
+  EXPECT_GT(colorSum, 0u) << "colorSum: " << colorSum;
+  EXPECT_GT(colorSum2, 0u) << "colorSum2: " << colorSum2;
+  EXPECT_GT(colorSum, colorSum2 + 10000000) <<
+      "colorSum: " << colorSum << ", " <<
+      "colorSum2: " << colorSum2;
+
+  delete[] img;
+  delete[] img2;
+}
+
 /////////////////////////////////////////////////
 TEST_F(CameraSensor, 16bit)
 {

worlds/lensflare_wideangle_cam.world

@@ -385,5 +385,64 @@
       </link>
     </model>
 
+    <!-- model with two wide-angle cameras with slow update rates, with and without lens flare plugin -->
+    <model name="slow_wide_angle_cameras">
+      <static>true</static>
+      <pose>0 12 11.0 0 0 0</pose>
+      <link name="link">
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="camera_sensor_lensflare" type="wideanglecamera">
+          <camera>
+            <horizontal_fov>3.14159</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>equidistant</type>
+              <scale_to_hfov>true</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>1</update_rate>
+          <visualize>true</visualize>
+          <plugin name="lensflare" filename="libLensFlareSensorPlugin.so"/>
+        </sensor>
+        <sensor name="camera_sensor_without_lensflare" type="wideanglecamera">
+          <camera>
+            <horizontal_fov>3.14159</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>equidistant</type>
+              <scale_to_hfov>true</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>1</update_rate>
+          <visualize>true</visualize>
+        </sensor>
+      </link>
+    </model>
   </world>
 </sdf>