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</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</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/EntityComponentManager.cc

@@ -808,6 +808,14 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity,
 
   // Find the entity in the message
   auto entIter = _msg.mutable_entities()->find(_entity);
+  if (entIter == _msg.mutable_entities()->end())
+  {
+    msgs::SerializedEntityMap ent;
+    ent.set_id(_entity);
+    entIter =
+      (_msg.mutable_entities())->insert({static_cast<uint64_t>(_entity), ent})
+      .first;
+  }
 
   auto it = this->removedComponents.find(_entity);
   for (uint64_t i = 0; i < nEntityKeys; ++i)

src/rendering/RenderUtil.cc

@@ -262,6 +262,11 @@ 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
+  public:std::unordered_map<Entity,
+      std::tuple<double, components::TemperatureRangeInfo>> thermalCameraData;
 };
 
 //////////////////////////////////////////////////
@@ -278,6 +283,49 @@ 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
+        double resolution = 0.0;
+        components::TemperatureRangeInfo range;
+        range.min = 0;
+        range.max = std::numeric_limits<double>::max();
+
+        // 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 +415,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 +430,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 +784,25 @@ 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);
+      double minTemp = std::get<1>(thermal.second).min.Kelvin();
+      double maxTemp = std::get<1>(thermal.second).max.Kelvin();
+      camera->SetMinTemperature(minTemp);
+      camera->SetMaxTemperature(maxTemp);
+    }
+  }
 }
 
 //////////////////////////////////////////////////

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,11 @@ 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}
+)
+
+

src/systems/thermal/ThermalSensor.cc

@@ -0,0 +1,93 @@
+/*
+ * 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 "ThermalSensor.hh"
+
+#include <limits>
+#include <string>
+
+#include <ignition/common/Util.hh>
+#include <ignition/plugin/Register.hh>
+
+#include "ignition/gazebo/components/ThermalCamera.hh"
+#include "ignition/gazebo/components/Temperature.hh"
+#include "ignition/gazebo/components/TemperatureRange.hh"
+#include "ignition/gazebo/EntityComponentManager.hh"
+#include "ignition/gazebo/Util.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+/// \brief Private Thermal sensor data class.
+class ignition::gazebo::systems::ThermalSensorPrivate
+{
+};
+
+//////////////////////////////////////////////////
+ThermalSensor::ThermalSensor() : System(),
+    dataPtr(std::make_unique<ThermalSensorPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+ThermalSensor::~ThermalSensor() = default;
+
+//////////////////////////////////////////////////
+void ThermalSensor::Configure(const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    gazebo::EntityComponentManager &_ecm,
+    gazebo::EventManager & /*_eventMgr*/)
+{
+  const std::string resolutionTag = "resolution";
+  const std::string minTempTag = "min_temp";
+  const std::string maxTempTag = "max_temp";
+
+  auto sensorComp = _ecm.Component<components::ThermalCamera>(_entity);
+  if (sensorComp == nullptr)
+  {
+    ignerr << "The thermal sensor system can only be attached to a "
+           << "thermal sensor" << std::endl;
+    return;
+  }
+
+  if (_sdf->HasElement(resolutionTag))
+  {
+     double resolution = _sdf->Get<double>(resolutionTag);
+    _ecm.CreateComponent(_entity,
+        components::TemperatureLinearResolution(resolution));
+  }
+
+  if (_sdf->HasElement(minTempTag) || _sdf->HasElement(maxTempTag))
+  {
+    double min = 0.0;
+    double max = std::numeric_limits<double>::max();
+    min = _sdf->Get<double>(minTempTag, min).first;
+    max = _sdf->Get<double>(maxTempTag, max).first;
+    components::TemperatureRangeInfo rangeInfo;
+    rangeInfo.min = min;
+    rangeInfo.max = max;
+    _ecm.CreateComponent(_entity, components::TemperatureRange(rangeInfo));
+  }
+}
+
+
+IGNITION_ADD_PLUGIN(ThermalSensor, System,
+  ThermalSensor::ISystemConfigure
+)
+
+IGNITION_ADD_PLUGIN_ALIAS(ThermalSensor, "ignition::gazebo::systems::ThermalSensor")