Support setting the background color for sensors

include/ignition/gazebo/rendering/RenderUtil.hh

@@ -86,11 +86,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \return Name of the rendering scene.
     public: std::string SceneName() const;
 
-    /// \brief Set background color of render window
+    /// \brief Set background color of render window. This will override
+    /// other sources, such as from SDF.
     /// \param[in] _color Color of render window background
     public: void SetBackgroundColor(const math::Color &_color);
 
-    /// \brief Set ambient light of render window
+    /// \brief Set ambient light of render window. This will override
+    /// other sources, such as from SDF.
     /// \param[in] _ambient Color of ambient light
     public: void SetAmbientLight(const math::Color &_ambient);
 

src/rendering/RenderUtil.cc

@@ -104,11 +104,15 @@ class ignition::gazebo::RenderUtilPrivate
   /// \brief Name of scene
   public: std::string sceneName = "scene";
 
-  /// \brief Scene background color
-  public: math::Color backgroundColor = math::Color::Black;
+  /// \brief Scene background color. This is optional because a <scene> is
+  /// always present, which has a default background color value. This
+  /// backgroundColor variable is used to override the <scene> value.
+  public: std::optional<math::Color> backgroundColor;
 
-  /// \brief Ambient color
-  public: math::Color ambientLight = math::Color(1.0, 1.0, 1.0, 1.0);
+  /// \brief Ambient color. This is optional because an <scene> is always
+  /// present, which has a default ambient light value. This ambientLight
+  /// variable is used to override the <scene> value.
+  public: std::optional<math::Color> ambientLight;
 
   /// \brief Scene manager
   public: SceneManager sceneManager;
@@ -355,8 +359,16 @@ void RenderUtil::Update()
   // extend the sensor system to support mutliple scenes in the future
   for (auto &scene : newScenes)
   {
-    this->dataPtr->scene->SetAmbientLight(scene.Ambient());
-    this->dataPtr->scene->SetBackgroundColor(scene.Background());
+    // Only set the ambient color if the RenderUtil::SetBackgroundColor
+    // was not called.
+    if (!this->dataPtr->ambientLight)
+      this->dataPtr->scene->SetAmbientLight(scene.Ambient());
+
+    // Only set the background color if the RenderUtil::SetBackgroundColor
+    // was not called.
+    if (!this->dataPtr->backgroundColor)
+      this->dataPtr->scene->SetBackgroundColor(scene.Background());
+
     if (scene.Grid() && !this->dataPtr->enableSensors)
       this->ShowGrid();
     // only one scene so break
@@ -1203,8 +1215,17 @@ void RenderUtil::Init()
         this->dataPtr->engine->CreateScene(this->dataPtr->sceneName);
     if (this->dataPtr->scene)
     {
-      this->dataPtr->scene->SetAmbientLight(this->dataPtr->ambientLight);
-      this->dataPtr->scene->SetBackgroundColor(this->dataPtr->backgroundColor);
+      if (this->dataPtr->ambientLight)
+      {
+        this->dataPtr->scene->SetAmbientLight(
+            *this->dataPtr->ambientLight);
+      }
+
+      if (this->dataPtr->backgroundColor)
+      {
+        this->dataPtr->scene->SetBackgroundColor(
+            *this->dataPtr->backgroundColor);
+      }
     }
   }
   this->dataPtr->sceneManager.SetScene(this->dataPtr->scene);

src/systems/sensors/Sensors.cc

@@ -164,6 +164,12 @@ class ignition::gazebo::systems::SensorsPrivate
 
   /// \brief Stop the rendering thread
   public: void Stop();
+
+  /// \brief Use to optionally set the background color.
+  public: std::optional<math::Color> backgroundColor;
+
+  /// \brief Use to optionally set the ambient light.
+  public: std::optional<math::Color> ambientLight;
 };
 
 //////////////////////////////////////////////////
@@ -184,6 +190,10 @@ void SensorsPrivate::WaitForInit()
     {
       // Only initialize if there are rendering sensors
       igndbg << "Initializing render context" << std::endl;
+      if (this->backgroundColor)
+        this->renderUtil.SetBackgroundColor(*this->backgroundColor);
+      if (this->ambientLight)
+        this->renderUtil.SetAmbientLight(*this->ambientLight);
       this->renderUtil.Init();
       this->scene = this->renderUtil.Scene();
       this->initialized = true;
@@ -331,10 +341,19 @@ void Sensors::Configure(const Entity &/*_id*/,
     EventManager &_eventMgr)
 {
   igndbg << "Configuring Sensors system" << std::endl;
+
   // Setup rendering
   std::string engineName =
       _sdf->Get<std::string>("render_engine", "ogre2").first;
 
+  // Get the background color, if specified.
+  if (_sdf->HasElement("background_color"))
+    this->dataPtr->backgroundColor = _sdf->Get<math::Color>("background_color");
+
+  // Get the ambient light, if specified.
+  if (_sdf->HasElement("ambient_light"))
+    this->dataPtr->ambientLight = _sdf->Get<math::Color>("ambient_light");
+
   this->dataPtr->renderUtil.SetEngineName(engineName);
   this->dataPtr->renderUtil.SetEnableSensors(true,
       std::bind(&Sensors::CreateSensor, this,

src/systems/sensors/Sensors.hh

@@ -36,7 +36,19 @@ namespace systems
   class SensorsPrivate;
 
   /// \class Sensors Sensors.hh ignition/gazebo/systems/Sensors.hh
-  /// \brief TODO(louise) Have one system for all sensors, or one per
+  /// \brief A system that manages sensors.
+  ///
+  /// ## System Parameters
+  ///
+  /// - `<render_engine>` Name of the render engine, such as 'ogre' or 'ogre2'.
+  /// - `<background_color>` Color used for the scene's background. This
+  /// will override the background color specified in a world's SDF <scene>
+  /// element. This background color is used by sensors, not the GUI.
+  /// - `<ambient_light>` Color used for the scene's ambient light. This
+  /// will override the ambient value specified in a world's SDF <scene>
+  /// element. This ambient light is used by sensors, not the GUI.
+  ///
+  /// \TODO(louise) Have one system for all sensors, or one per
   /// sensor / sensor type?
   class Sensors:
     public System,

test/integration/CMakeLists.txt

@@ -52,6 +52,7 @@ set(tests
 
 # Tests that require a valid display
 set(tests_needing_display
+  camera_sensor_background.cc
   camera_video_record_system.cc
   depth_camera.cc
   gpu_lidar.cc

test/integration/camera_sensor_background.cc

@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2021 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 <gtest/gtest.h>
+
+#include <string>
+
+#include <ignition/transport/Node.hh>
+#include <ignition/utilities/ExtraTestMacros.hh>
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/test_config.hh"
+
+#include "../helpers/EnvTestFixture.hh"
+
+using namespace ignition;
+using namespace std::chrono_literals;
+
+std::mutex mutex;
+int cbCount = 0;
+
+//////////////////////////////////////////////////
+class CameraSensorBackgroundFixture :
+  public InternalFixture<InternalFixture<::testing::Test>>
+{
+};
+
+/////////////////////////////////////////////////
+void cameraCb(const msgs::Image & _msg)
+{
+  ASSERT_EQ(msgs::PixelFormatType::RGB_INT8,
+      _msg.pixel_format_type());
+
+  for (unsigned int y = 0; y < _msg.height(); ++y)
+  {
+    for (unsigned int x = 0; x < _msg.width(); ++x)
+    {
+      // The "/test/worlds/camera_sensor_empty_scene.sdf" world has set a
+      // background color of 1,0,0,1. So, all the pixels returned by the
+      // camera should be red.
+      unsigned char r = _msg.data()[y * _msg.step() + x*3];
+      EXPECT_EQ(255, static_cast<int>(r));
+
+      unsigned char g = _msg.data()[y * _msg.step() + x*3+1];
+      EXPECT_EQ(0, static_cast<int>(g));
+
+      unsigned char b = _msg.data()[y * _msg.step() + x*3+2];
+      EXPECT_EQ(0, static_cast<int>(b));
+    }
+  }
+  std::lock_guard<std::mutex> lock(mutex);
+  cbCount++;
+}
+
+/////////////////////////////////////////////////
+// Test the ability to set the background color using the sensor system
+// plugin.
+TEST_F(CameraSensorBackgroundFixture,
+    IGN_UTILS_TEST_DISABLED_ON_MAC(RedBackground))
+{
+  // Start server
+  gazebo::ServerConfig serverConfig;
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/camera_sensor_empty_scene.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // subscribe to the camera topic
+  transport::Node node;
+  cbCount = 0;
+  node.Subscribe("/camera", &cameraCb);
+
+  // Run server and verify that we are receiving a message
+  // from the depth camera
+  server.Run(true, 100, false);
+
+  int i = 0;
+  while (i < 100 && cbCount <= 0)
+  {
+    common::Time::Sleep(common::Time(0.1));
+    i++;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex);
+  EXPECT_GE(cbCount, 1);
+}

test/worlds/camera_sensor_empty_scene.sdf

@@ -0,0 +1,55 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="sensors">
+    <physics name="1ms" type="ignored">
+      <max_step_size>.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+      <background_color>1 0 0 1</background_color>
+    </plugin>
+
+    <model name="camera">
+      <static>true</static>
+      <pose>0 0 1.0 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="camera" type="camera">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+          <update_rate>30</update_rate>
+          <topic>camera</topic>
+        </sensor>
+      </link>
+    </model>
+  </world>
+</sdf>