Add thermal sensor system for configuring thermal camera properties

src/rendering/RenderUtil.cc

@@ -800,19 +800,19 @@ void RenderUtil::Update()
     if (camera)
     {
       double resolution = std::get<0>(thermal.second);
+
       if (resolution > 0.0)
       {
         camera->SetLinearResolution(resolution);
       }
       else
       {
         ignwarn << "Unable to set thermal camera temperature linear resolution."
-                << " Value must be greater than 0." << std::endl;
+                << " Value must be greater than 0. Using the default value: "
+                << camera->LinearResolution() << ". " << std::endl;
       }
-
       double minTemp = std::get<1>(thermal.second).min.Kelvin();
       double maxTemp = std::get<1>(thermal.second).max.Kelvin();
-
       if (maxTemp >= minTemp)
       {
         camera->SetMinTemperature(minTemp);
@@ -821,9 +821,9 @@ void RenderUtil::Update()
       else
       {
         ignwarn << "Unable to set thermal camera temperature range."
-                << "Max temperature must be greater or equal to min"
-                << std::endl;
-
+                << "Max temperature must be greater or equal to min. "
+                << "Using the default values : [" << camera->MinTemperature()
+                << ", " << camera->MaxTemperature() << "]." << std::endl;
       }
     }
   }
@@ -1155,6 +1155,31 @@ void RenderUtilPrivate::CreateRenderingEntities(
             visual.SetMaterial(material->Data());
           }
 
+          if (auto temp = _ecm.Component<components::Temperature>(_entity))
+          {
+            // get the uniform temperature for the entity
+            this->entityTemp[_entity] =
+              std::make_tuple<float, float, std::string>(
+                  temp->Data().Kelvin(), 0.0, "");
+          }
+          else
+          {
+            // entity doesn't have a uniform temperature. Check if it has
+            // a heat signature with an associated temperature range
+            auto heatSignature =
+              _ecm.Component<components::SourceFilePath>(_entity);
+            auto tempRange =
+               _ecm.Component<components::TemperatureRange>(_entity);
+            if (heatSignature && tempRange)
+            {
+              this->entityTemp[_entity] =
+                std::make_tuple<float, float, std::string>(
+                    tempRange->Data().min.Kelvin(),
+                    tempRange->Data().max.Kelvin(),
+                    std::string(heatSignature->Data()));
+            }
+          }
+
           this->newVisuals.push_back(
               std::make_tuple(_entity, visual, _parent->Data()));
           return true;

test/integration/thermal_system.cc

@@ -51,6 +51,27 @@ class ThermalTest : public ::testing::Test
   }
 };
 
+
+std::mutex g_mutex;
+std::vector<msgs::Image> g_imageMsgs;
+unsigned char *g_image = nullptr;
+
+/////////////////////////////////////////////////
+void thermalCb(const msgs::Image &_msg)
+{
+  std::lock_guard<std::mutex> g_lock(g_mutex);
+  g_imageMsgs.push_back(_msg);
+
+  unsigned int width = _msg.width();
+  unsigned int height = _msg.height();
+  unsigned int size = width * height * sizeof(unsigned char);
+  if (!g_image)
+  {
+    g_image = new unsigned char[size];
+  }
+  memcpy(g_image, _msg.data().c_str(), size);
+}
+
 /////////////////////////////////////////////////
 TEST_F(ThermalTest, TemperatureComponent)
 {
@@ -192,7 +213,7 @@ TEST_F(ThermalTest, ThermalSensorSystem)
 
   double resolution = 0;
   double minTemp = std::numeric_limits<double>::max();
-  double maxTemp = 0u;
+  double maxTemp = 0.0;
   std::string name;
   sdf::Sensor sensorSdf;
   testSystem.OnUpdate([&](const gazebo::UpdateInfo &,
@@ -228,13 +249,162 @@ TEST_F(ThermalTest, ThermalSensorSystem)
   // Run server
   server.Run(true, 1, false);
 
+  // verify camera properties from sdf
   EXPECT_EQ("thermal_camera_8bit", name);
   const sdf::Camera *cameraSdf = sensorSdf.CameraSensor();
   ASSERT_NE(nullptr, cameraSdf);
   EXPECT_EQ(320u, cameraSdf->ImageWidth());
   EXPECT_EQ(240u, cameraSdf->ImageHeight());
   EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat());
-  EXPECT_NEAR(3.0, resolution, 1e-6);
-  EXPECT_NEAR(253.15, minTemp, 1e-6);
-  EXPECT_NEAR(673.15, maxTemp, 1e-6);
+
+  // verify camera properties set through plugin
+  EXPECT_DOUBLE_EQ(3.0, resolution);
+  EXPECT_DOUBLE_EQ(253.15, minTemp);
+  EXPECT_DOUBLE_EQ(673.15, maxTemp);
+}
+
+/////////////////////////////////////////////////
+TEST_F(ThermalTest, ThermalSensorSystemInvalidConfig)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/thermal_invalid.sdf");
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system that checks for thermal component
+  test::Relay testSystem;
+
+  double resolution = 0;
+  double minTemp = std::numeric_limits<double>::max();
+  double maxTemp = 0.0;
+  std::map<std::string, math::Temperature> entityTemp;
+  std::string name;
+  sdf::Sensor sensorSdf;
+
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+    const gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Temperature, components::Name>(
+          [&](const ignition::gazebo::Entity &_id,
+              const components::Temperature *_temp,
+              const components::Name *_name) -> bool
+          {
+            // store temperature data
+            entityTemp[_name->Data()] = _temp->Data();
+
+            // verify temperature data belongs to a visual
+            EXPECT_NE(nullptr, _ecm.Component<components::Visual>(_id));
+
+            return true;
+          });
+    });
+  testSystem.OnUpdate([&](const gazebo::UpdateInfo &,
+    gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::ThermalCamera, components::Name>(
+          [&](const ignition::gazebo::Entity &_id,
+              const components::ThermalCamera *_sensor,
+              const components::Name *_name) -> bool
+          {
+            // store temperature data
+            sensorSdf = _sensor->Data();
+            name = _name->Data();
+
+            auto resolutionComp =
+                _ecm.Component<components::TemperatureLinearResolution>(
+                _id);
+            if (resolutionComp)
+              resolution = resolutionComp->Data();
+
+            auto temperatureRangeComp =
+                _ecm.Component<components::TemperatureRange>(_id);
+            if (temperatureRangeComp)
+            {
+              auto info = temperatureRangeComp->Data();
+              minTemp = info.min.Kelvin();
+              maxTemp = info.max.Kelvin();
+            }
+            return true;
+          });
+
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+
+  // subscribe to thermal topic
+  transport::Node node;
+  node.Subscribe("/thermal_camera_invalid/image", &thermalCb);
+
+  // Run server
+  server.Run(true, 1, false);
+
+  // verify camera properties from sdf
+  unsigned int width = 320u;
+  unsigned int height = 240u;
+  EXPECT_EQ("thermal_camera_invalid", name);
+  const sdf::Camera *cameraSdf = sensorSdf.CameraSensor();
+  ASSERT_NE(nullptr, cameraSdf);
+  EXPECT_EQ(width, cameraSdf->ImageWidth());
+  EXPECT_EQ(height, cameraSdf->ImageHeight());
+  EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat());
+
+  // verify camera properties set through plugin
+  // the resolution, min and max are invalid range values. Ign-gazebo should
+  // print out warnings and use default values
+  EXPECT_DOUBLE_EQ(0.0, resolution);
+  EXPECT_DOUBLE_EQ(999.0, minTemp);
+  EXPECT_DOUBLE_EQ(-590.0, maxTemp);
+
+  // verify temperature of heat source
+  const std::string sphereVisual = "sphere_visual";
+  const std::string cylinderVisual = "cylinder_visual";
+  EXPECT_EQ(2u, entityTemp.size());
+  ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end());
+  ASSERT_TRUE(entityTemp.find(cylinderVisual) != entityTemp.end());
+  EXPECT_DOUBLE_EQ(600.0, entityTemp[sphereVisual].Kelvin());
+  // the user specified temp is larger than the max value representable by an
+  // 8 bit 3 degree resolution camera - this value should be clamped
+  EXPECT_DOUBLE_EQ(800.0, entityTemp[cylinderVisual].Kelvin());
+
+  // Run server
+  server.Run(true, 10, false);
+
+  // wait for image
+  bool received = false;
+  for (unsigned int i = 0; i < 20; ++i)
+  {
+    {
+      std::lock_guard<std::mutex> lock(g_mutex);
+      received = !g_imageMsgs.empty();
+    }
+    if (received)
+      break;
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  EXPECT_TRUE(received);
+
+  // check temperature in actual image output
+  {
+    std::lock_guard<std::mutex> lock(g_mutex);
+    unsigned int leftIdx = height * 0.5 * width;
+    unsigned int rightIdx = leftIdx + width-1;
+    unsigned int defaultResolution = 3u;
+    unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution;
+    unsigned int sphereTemp = g_image[rightIdx] * defaultResolution;
+    // default resolution, min, max values used so we should get correct
+    // temperature value
+    EXPECT_EQ(600u, sphereTemp);
+    // 8 bit 3 degree resolution camera - this value should be clamped
+    // in the image output to:
+    //     2^(bitDepth) - 1 * resolution = 2^8 - 1 * 3 = 765
+    EXPECT_EQ(765u, cylinderTemp);
+  }
+
+  g_imageMsgs.clear();
+  delete [] g_image;
+  g_image = nullptr;
 }

test/worlds/thermal.sdf

@@ -351,7 +351,6 @@
             <max_temp>673.15</max_temp>
             <resolution>3.0</resolution>
           </plugin>
-
         </sensor>
       </link>
       <static>true</static>