Add thermal sensor system for configuring thermal camera properties

examples/worlds/thermal_camera.sdf

@@ -104,6 +104,18 @@
         <topic>thermal_camera</topic>
         <topic_picker>false</topic_picker>
       </plugin>
+      <plugin filename="ImageDisplay" name="Image Display 8 Bit">
+        <ignition-gui>
+          <title>Thermal camera 8 Bit</title>
+          <property key="state" type="string">floating</property>
+          <property type="double" key="width">350</property>
+          <property type="double" key="height">315</property>
+          <property type="double" key="x">500</property>
+        </ignition-gui>
+        <topic>thermal_camera_8bit/image</topic>
+        <topic_picker>false</topic_picker>
+      </plugin>
+
     </gui>
 
     <light type="directional" name="sun">
@@ -230,6 +242,7 @@
             <image>
               <width>320</width>
               <height>240</height>
+              <format>L16</format>
             </image>
             <clip>
               <near>0.1</near>
@@ -245,6 +258,55 @@
       <static>true</static>
     </model>
 
+    <model name="thermal_camera_8bit">
+      <pose>4.5 0 0.5 0.0 0.0 3.14</pose>
+      <link name="link">
+        <pose>0.05 0.05 0.05 0 0 0</pose>
+        <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="thermal_camera_8bit" type="thermal">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+              <format>L8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <topic>thermal_camera_8bit/image</topic>
+          <plugin
+            filename="ignition-gazebo-thermal-sensor-system"
+            name="ignition::gazebo::systems::ThermalSensor">
+            <min_temp>253.15</min_temp>
+            <max_temp>673.15</max_temp>
+            <resolution>3.0</resolution>
+          </plugin>
+
+        </sensor>
+      </link>
+      <static>true</static>
+    </model>
+
+
     <include>
       <pose>1 0 0 0 0 1.570796</pose>
       <name>rescue_randy</name>

include/ignition/gazebo/components/Temperature.hh

@@ -35,6 +35,12 @@ namespace components
   using Temperature = Component<math::Temperature, class TemperatureTag>;
   IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Temperature",
       Temperature)
+
+  /// \brief A component that stores temperature linear resolution in Kelvin
+  using TemperatureLinearResolution = Component<double, class TemperatureTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT(
+      "ign_gazebo_components.TemperatureLinearResolution",
+      TemperatureLinearResolution)
 }
 }
 }

include/ignition/gazebo/rendering/RenderUtil.hh

@@ -66,6 +66,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \return Pointer to the scene
     public: rendering::ScenePtr Scene() const;
 
+    /// \brief Helper Update function for updating the scene
+    /// \param[in] _info Sim update info
+    /// \param[in] _ecm Mutable reference to the entity component manager
+    public: void UpdateECM(const UpdateInfo &_info,
+                           EntityComponentManager &_ecm);
+
     /// \brief Helper PostUpdate function for updating the scene
     public: void UpdateFromECM(const UpdateInfo &_info,
                                const EntityComponentManager &_ecm);

src/rendering/RenderUtil.cc

@@ -262,6 +262,12 @@ class ignition::gazebo::RenderUtilPrivate
   /// \brief A map of created collision entities and if they are currently
   /// visible
   public: std::map<Entity, bool> viewingCollisions;
+
+  /// \brief A map of entity id to thermal camera sensor configuration
+  /// properties. The elements in the tuple are:
+  /// <resolution, temperature range (min, max)>
+  public:std::unordered_map<Entity,
+      std::tuple<double, components::TemperatureRangeInfo>> thermalCameraData;
 };
 
 //////////////////////////////////////////////////
@@ -278,6 +284,53 @@ rendering::ScenePtr RenderUtil::Scene() const
   return this->dataPtr->scene;
 }
 
+//////////////////////////////////////////////////
+void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
+                           EntityComponentManager &_ecm)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->updateMutex);
+  // Update thermal cameras
+  _ecm.Each<components::ThermalCamera>(
+      [&](const Entity &_entity,
+        const components::ThermalCamera *)->bool
+      {
+        // set properties from thermal sensor plugin
+        // Set defaults to invaid values so we know they have not been set.
+        // set UpdateECM(). We check for valid values first before setting
+        // these thermal camera properties..
+        double resolution = 0.0;
+        components::TemperatureRangeInfo range;
+        range.min = std::numeric_limits<double>::max();
+        range.max = 0;
+
+        // resolution
+        auto resolutionComp =
+          _ecm.Component<components::TemperatureLinearResolution>(_entity);
+        if (resolutionComp != nullptr)
+        {
+          resolution = resolutionComp->Data();
+          _ecm.RemoveComponent<components::TemperatureLinearResolution>(
+              _entity);
+        }
+
+        // min / max temp
+        auto tempRangeComp =
+          _ecm.Component<components::TemperatureRange>(_entity);
+        if (tempRangeComp != nullptr)
+        {
+          range = tempRangeComp->Data();
+          _ecm.RemoveComponent<components::TemperatureRange>(_entity);
+        }
+
+        if (resolutionComp || tempRangeComp)
+        {
+          this->dataPtr->thermalCameraData[_entity] =
+              std::make_tuple(resolution, range);
+        }
+        return true;
+      });
+}
+
 //////////////////////////////////////////////////
 void RenderUtil::UpdateFromECM(const UpdateInfo &_info,
                                const EntityComponentManager &_ecm)
@@ -367,6 +420,7 @@ void RenderUtil::Update()
   auto actorAnimationData = std::move(this->dataPtr->actorAnimationData);
   auto entityTemp = std::move(this->dataPtr->entityTemp);
   auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks);
+  auto thermalCameraData = std::move(this->dataPtr->thermalCameraData);
 
   this->dataPtr->newScenes.clear();
   this->dataPtr->newModels.clear();
@@ -381,6 +435,7 @@ void RenderUtil::Update()
   this->dataPtr->actorAnimationData.clear();
   this->dataPtr->entityTemp.clear();
   this->dataPtr->newCollisionLinks.clear();
+  this->dataPtr->thermalCameraData.clear();
 
   this->dataPtr->markerManager.Update();
 
@@ -734,6 +789,44 @@ void RenderUtil::Update()
       }
     }
   }
+
+  // update thermal camera
+  for (const auto &thermal : this->dataPtr->thermalCameraData)
+  {
+    Entity id = thermal.first;
+    rendering::ThermalCameraPtr camera =
+        std::dynamic_pointer_cast<rendering::ThermalCamera>(
+        this->dataPtr->sceneManager.NodeById(id));
+    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. 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);
+        camera->SetMaxTemperature(maxTemp);
+      }
+      else
+      {
+        ignwarn << "Unable to set thermal camera temperature range."
+                << "Max temperature must be greater or equal to min. "
+                << "Using the default values : [" << camera->MinTemperature()
+                << ", " << camera->MaxTemperature() << "]." << std::endl;
+      }
+    }
+  }
 }
 
 //////////////////////////////////////////////////
@@ -1062,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;

src/systems/sensors/Sensors.cc

@@ -412,6 +412,17 @@ void Sensors::Configure(const Entity &/*_id*/,
   this->dataPtr->Run();
 }
 
+//////////////////////////////////////////////////
+void Sensors::Update(const UpdateInfo &_info,
+                     EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("Sensors::Update");
+  if (this->dataPtr->running && this->dataPtr->initialized)
+  {
+    this->dataPtr->renderUtil.UpdateECM(_info, _ecm);
+  }
+}
+
 //////////////////////////////////////////////////
 void Sensors::PostUpdate(const UpdateInfo &_info,
                          const EntityComponentManager &_ecm)
@@ -592,6 +603,7 @@ std::string Sensors::CreateSensor(const Entity &_entity,
 
 IGNITION_ADD_PLUGIN(Sensors, System,
   Sensors::ISystemConfigure,
+  Sensors::ISystemUpdate,
   Sensors::ISystemPostUpdate
 )
 

src/systems/sensors/Sensors.hh

@@ -42,6 +42,7 @@ namespace systems
   class IGNITION_GAZEBO_VISIBLE Sensors:
     public System,
     public ISystemConfigure,
+    public ISystemUpdate,
     public ISystemPostUpdate
   {
     /// \brief Constructor
@@ -56,6 +57,10 @@ namespace systems
                            EntityComponentManager &_ecm,
                            EventManager &_eventMgr) final;
 
+    // Documentation inherited
+    public: void Update(const UpdateInfo &_info,
+                        EntityComponentManager &_ecm) final;
+
     // Documentation inherited
     public: void PostUpdate(const UpdateInfo &_info,
                             const EntityComponentManager &_ecm) final;

src/systems/thermal/CMakeLists.txt

@@ -5,3 +5,9 @@ gz_add_system(thermal
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
 )
 
+gz_add_system(thermal-sensor
+  SOURCES
+    ThermalSensor.cc
+  PUBLIC_LINK_LIBS
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)