Merged in thermal (pull request #514)

Thermal camera Part 3: Sensor Approved-by: Louise Poubel <[email protected]>
gazebosim · Feb 6, 2020 · a2b4ac8 · a2b4ac8
2 parents e6d8dee + a731091
commit a2b4ac8
Show file tree

Hide file tree

Showing 10 changed files with 161 additions and 38 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -98,6 +98,7 @@ ign_find_package(ignition-sensors3 REQUIRED
     logical_camera
     magnetometer
     depth_camera
+    thermal_camera
 )
 set(IGN_SENSORS_VER ${ignition-sensors3_VERSION_MAJOR})
 

diff --git a/Changelog.md b/Changelog.md
@@ -2,6 +2,11 @@
 
 ### Ignition Gazebo 3.X.X
 
+1. Add support for thermal camera
+    * [Pull Request 512](https://bitbucket.org/ignitionrobotics/ign-gazebo/pull-requests/512)
+    * [Pull Request 513](https://bitbucket.org/ignitionrobotics/ign-gazebo/pull-requests/513)
+    * [Pull Request 514](https://bitbucket.org/ignitionrobotics/ign-gazebo/pull-requests/514)
+
 ### Ignition Gazebo 3.0.0 (2019-12-10)
 
 1. Remove <emissive> sdf element from visuals that do not emit light in the example worlds

diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf
@@ -90,7 +90,6 @@
         <topic>/world/thermal_camera/stats</topic>
       </plugin>
 
-      <!--
       <plugin filename="ImageDisplay" name="Image Display">
         <ignition-gui>
           <title>Thermal camera</title>
@@ -101,7 +100,6 @@
         <topic>thermal_camera</topic>
         <topic_picker>false</topic_picker>
       </plugin>
-      -->
     </gui>
 
     <light type="directional" name="sun">
@@ -282,7 +280,6 @@
       </link>
     </model>
 
-    <!--
     <model name="thermal_camera">
       <pose>4.5 0 0.5 0.0 0.0 3.14</pose>
       <link name="link">
@@ -321,7 +318,6 @@
       </link>
       <static>true</static>
     </model>
-    -->
 
   </world>
 </sdf>
diff --git a/include/ignition/gazebo/components/ThermalCamera.hh b/include/ignition/gazebo/components/ThermalCamera.hh
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2020 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.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_THERMALCAMERA_HH_
+#define IGNITION_GAZEBO_COMPONENTS_THERMALCAMERA_HH_
+
+#include <sdf/Sensor.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component type that contains a Thermal camera sensor,
+  /// sdf::Sensor, information.
+  using ThermalCamera = Component<sdf::Sensor, class ThermalCameraTag,
+      serializers::SensorSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ThermalCamera",
+      ThermalCamera)
+}
+}
+}
+}
+#endif
diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -806,7 +806,8 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in)
   }
   else if (_in.Type() == sdf::SensorType::CAMERA ||
            _in.Type() == sdf::SensorType::DEPTH_CAMERA ||
-           _in.Type() == sdf::SensorType::RGBD_CAMERA)
+           _in.Type() == sdf::SensorType::RGBD_CAMERA ||
+           _in.Type() == sdf::SensorType::THERMAL_CAMERA)
   {
     if (_in.CameraSensor())
     {
@@ -1024,7 +1025,8 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in)
   }
   else if (out.Type() == sdf::SensorType::CAMERA ||
            out.Type() == sdf::SensorType::DEPTH_CAMERA ||
-           out.Type() == sdf::SensorType::RGBD_CAMERA)
+           out.Type() == sdf::SensorType::RGBD_CAMERA ||
+           out.Type() == sdf::SensorType::THERMAL_CAMERA)
   {
     sdf::Camera sensor;
 

diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -59,6 +59,7 @@
 #include "ignition/gazebo/components/Scene.hh"
 #include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/Static.hh"
+#include "ignition/gazebo/components/ThermalCamera.hh"
 #include "ignition/gazebo/components/ThreadPitch.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/WindMode.hh"
@@ -531,6 +532,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor)
     this->dataPtr->ecm->CreateComponent(sensorEntity,
         components::RgbdCamera(*_sensor));
   }
+  else if (_sensor->Type() == sdf::SensorType::THERMAL_CAMERA)
+  {
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+        components::ThermalCamera(*_sensor));
+  }
   else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE)
   {
     this->dataPtr->ecm->CreateComponent(sensorEntity,

diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
@@ -56,6 +56,7 @@
 #include "ignition/gazebo/components/RgbdCamera.hh"
 #include "ignition/gazebo/components/Scene.hh"
 #include "ignition/gazebo/components/Temperature.hh"
+#include "ignition/gazebo/components/ThermalCamera.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
@@ -355,31 +356,31 @@ void RenderUtil::Update()
     {
       for (const auto &sensor : newSensors)
       {
-         Entity entity = std::get<0>(sensor);
-         const sdf::Sensor &dataSdf = std::get<1>(sensor);
-         Entity parent = std::get<2>(sensor);
-
-         // two sensors with the same name cause conflicts. We'll need to use
-         // scoped names
-         // TODO(anyone) do this in ign-sensors?
-         auto parentNode = this->dataPtr->sceneManager.NodeById(parent);
-         if (!parentNode)
-         {
-           ignerr << "Failed to create sensor with name[" << dataSdf.Name()
-                  << "] for entity [" << entity
-                  << "]. Parent not found with ID[" << parent << "]."
-                  << std::endl;
-           continue;
-         }
-
-         std::string sensorName =
-             this->dataPtr->createSensorCb(dataSdf, parentNode->Name());
-         // Add to the system's scene manager
-         if (!this->dataPtr->sceneManager.AddSensor(entity, sensorName, parent))
-         {
-           ignerr << "Failed to create sensor [" << sensorName << "]"
-                  << std::endl;
-         }
+        Entity entity = std::get<0>(sensor);
+        const sdf::Sensor &dataSdf = std::get<1>(sensor);
+        Entity parent = std::get<2>(sensor);
+
+        // two sensors with the same name cause conflicts. We'll need to use
+        // scoped names
+        // TODO(anyone) do this in ign-sensors?
+        auto parentNode = this->dataPtr->sceneManager.NodeById(parent);
+        if (!parentNode)
+        {
+          ignerr << "Failed to create sensor with name[" << dataSdf.Name()
+                 << "] for entity [" << entity
+                 << "]. Parent not found with ID[" << parent << "]."
+                 << std::endl;
+          continue;
+        }
+
+        std::string sensorName =
+            this->dataPtr->createSensorCb(dataSdf, parentNode->Name());
+        // Add to the system's scene manager
+        if (!this->dataPtr->sceneManager.AddSensor(entity, sensorName, parent))
+        {
+          ignerr << "Failed to create sensor [" << sensorName << "]"
+                 << std::endl;
+        }
       }
     }
   }
@@ -420,6 +421,7 @@ void RenderUtil::Update()
       tf.second.erase("actorPose");
       actorMesh->SetSkeletonLocalTransforms(tf.second);
     }
+
     // set visual temperature
     for (auto &temp : entityTemp)
     {
@@ -462,6 +464,7 @@ void RenderUtilPrivate::CreateRenderingEntities(
   const std::string cameraSuffix{"/image"};
   const std::string depthCameraSuffix{"/depth_image"};
   const std::string rgbdCameraSuffix{""};
+  const std::string thermalCameraSuffix{""};
   const std::string gpuLidarSuffix{"/scan"};
 
   // Treat all pre-existent entities as new at startup
@@ -623,6 +626,17 @@ void RenderUtilPrivate::CreateRenderingEntities(
                          gpuLidarSuffix);
             return true;
           });
+
+      // Create thermal camera
+      _ecm.Each<components::ThermalCamera, components::ParentEntity>(
+        [&](const Entity &_entity,
+            const components::ThermalCamera *_thermalCamera,
+            const components::ParentEntity *_parent)->bool
+          {
+            addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(),
+                         thermalCameraSuffix);
+            return true;
+          });
     }
     this->initialized = true;
   }
@@ -773,6 +787,17 @@ void RenderUtilPrivate::CreateRenderingEntities(
                          gpuLidarSuffix);
             return true;
           });
+
+      // Create thermal camera
+      _ecm.EachNew<components::ThermalCamera, components::ParentEntity>(
+        [&](const Entity &_entity,
+            const components::ThermalCamera *_thermalCamera,
+            const components::ParentEntity *_parent)->bool
+          {
+            addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(),
+                         thermalCameraSuffix);
+            return true;
+          });
     }
   }
 }
@@ -872,6 +897,16 @@ void RenderUtilPrivate::UpdateRenderingEntities(
         this->entityPoses[_entity] = _pose->Data();
         return true;
       });
+
+  // Update thermal cameras
+  _ecm.Each<components::ThermalCamera, components::Pose>(
+      [&](const Entity &_entity,
+        const components::ThermalCamera *,
+        const components::Pose *_pose)->bool
+      {
+        this->entityPoses[_entity] = _pose->Data();
+        return true;
+      });
 }
 
 //////////////////////////////////////////////////
@@ -940,6 +975,14 @@ void RenderUtilPrivate::RemoveRenderingEntities(
         this->removeEntities[_entity] = _info.iterations;
         return true;
       });
+
+  // thermal cameras
+  _ecm.EachRemoved<components::ThermalCamera>(
+    [&](const Entity &_entity, const components::ThermalCamera *)->bool
+      {
+        this->removeEntities[_entity] = _info.iterations;
+        return true;
+      });
 }
 
 /////////////////////////////////////////////////

diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt
@@ -7,6 +7,7 @@ gz_add_system(sensors
     ignition-sensors${IGN_SENSORS_VER}::camera
     ignition-sensors${IGN_SENSORS_VER}::gpu_lidar
     ignition-sensors${IGN_SENSORS_VER}::depth_camera
+    ignition-sensors${IGN_SENSORS_VER}::thermal_camera
     ${PROJECT_LIBRARY_TARGET_NAME}-rendering
 )
 
diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc
@@ -28,12 +28,16 @@
 #include <ignition/rendering/Scene.hh>
 #include <ignition/sensors/CameraSensor.hh>
 #include <ignition/sensors/RenderingSensor.hh>
+#include <ignition/sensors/ThermalCameraSensor.hh>
 #include <ignition/sensors/Manager.hh>
 
+#include "ignition/gazebo/components/Atmosphere.hh"
 #include "ignition/gazebo/components/Camera.hh"
 #include "ignition/gazebo/components/DepthCamera.hh"
 #include "ignition/gazebo/components/GpuLidar.hh"
 #include "ignition/gazebo/components/RgbdCamera.hh"
+#include "ignition/gazebo/components/ThermalCamera.hh"
+#include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/Events.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 
@@ -66,6 +70,10 @@ class ignition::gazebo::systems::SensorsPrivate
   /// generate sensor data
   public: rendering::ScenePtr scene;
 
+  /// \brief Temperature used by thermal camera. Defaults to temperature at
+  /// sea level
+  public: double ambientTemperature = 288.15;
+
   /// \brief Keep track of cameras, in case we need to handle stereo cameras.
   /// Key: Camera's parent scoped name
   /// Value: Pointer to camera
@@ -301,7 +309,7 @@ Sensors::~Sensors()
 //////////////////////////////////////////////////
 void Sensors::Configure(const Entity &/*_id*/,
     const std::shared_ptr<const sdf::Element> &_sdf,
-    EntityComponentManager &/*_ecm*/,
+    EntityComponentManager &_ecm,
     EventManager &_eventMgr)
 {
   igndbg << "Configuring Sensors system" << std::endl;
@@ -314,6 +322,19 @@ void Sensors::Configure(const Entity &/*_id*/,
       std::bind(&Sensors::CreateSensor, this,
       std::placeholders::_1, std::placeholders::_2));
 
+  // parse sensor-specific data
+  auto worldEntity = _ecm.EntityByComponents(components::World());
+  if (kNullEntity != worldEntity)
+  {
+    // temperature used by thermal camera
+    auto atmosphere = _ecm.Component<components::Atmosphere>(worldEntity);
+    if (atmosphere)
+    {
+      auto atmosphereSdf = atmosphere->Data();
+      this->dataPtr->ambientTemperature = atmosphereSdf.Temperature().Kelvin();
+    }
+  }
+
   this->dataPtr->stopConn = _eventMgr.Connect<events::Stop>(
       std::bind(&SensorsPrivate::Stop, this->dataPtr.get()));
 
@@ -339,7 +360,8 @@ void Sensors::PostUpdate(const UpdateInfo &_info,
       (_ecm.HasComponentType(components::Camera::typeId) ||
        _ecm.HasComponentType(components::DepthCamera::typeId) ||
        _ecm.HasComponentType(components::GpuLidar::typeId) ||
-       _ecm.HasComponentType(components::RgbdCamera::typeId)))
+       _ecm.HasComponentType(components::RgbdCamera::typeId) ||
+       _ecm.HasComponentType(components::ThermalCamera::typeId)))
   {
     igndbg << "Initialization needed" << std::endl;
     std::unique_lock<std::mutex> lock(this->dataPtr->renderMutex);
@@ -470,6 +492,13 @@ std::string Sensors::CreateSensor(const sdf::Sensor &_sdf,
     }
   }
 
+  // Sensor-specific settings
+  auto thermalSensor = dynamic_cast<sensors::ThermalCameraSensor *>(sensor);
+  if (nullptr != thermalSensor)
+  {
+    thermalSensor->SetAmbientTemperature(this->dataPtr->ambientTemperature);
+  }
+
   return sensor->Name();
 }
 

diff --git a/src/systems/thermal/Thermal.cc b/src/systems/thermal/Thermal.cc
@@ -56,11 +56,6 @@ void Thermal::Configure(const Entity &_entity,
   }
   double temperature = _sdf->Get<double>("temperature");
   _ecm.CreateComponent(_entity, components::Temperature(temperature));
-
-  // todo(anyone) This just prints out temperature data. Remove this msg later.
-  auto n = _ecm.Component<components::Name>(_entity);
-  ignmsg << "Thermal system: setting " << n->Data() << "'s temperature to "
-         << temperature << std::endl;
 }