Skip to content

Commit

Permalink
add heat signature option to thermal system (#498)
Browse files Browse the repository at this point in the history
Signed-off-by: Ashton Larkin <[email protected]>
  • Loading branch information
adlarkin committed Feb 2, 2021
1 parent d8b9d0c commit b4e152d
Show file tree
Hide file tree
Showing 8 changed files with 552 additions and 158 deletions.
119 changes: 29 additions & 90 deletions examples/worlds/thermal_camera.sdf
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<!--
Demo world consisting of a thermal camera sensor and 3 models with
Demo world consisting of a thermal camera sensor and several models with
different temperatures specified via the thermal system plugin.
-->

Expand Down Expand Up @@ -121,7 +121,14 @@
</light>

<atmosphere type="adiabatic">
<temperature>310</temperature>
<temperature>300</temperature>
<!--
This is a more exaggerated temperature gradient, which produces a
temperature range of ~11.5 kelvin for objects in the thermal camera
view. Typical temperature gradient is -0.0065 K/m which produces a
temperature range of 0.75 kelvin
-->
<temperature_gradient>0.1</temperature_gradient>
</atmosphere>

<model name="ground_plane">
Expand Down Expand Up @@ -157,7 +164,7 @@
</model>

<model name="box">
<pose>0 0 0.5 0 0 0</pose>
<pose>-1 1 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
Expand Down Expand Up @@ -192,93 +199,7 @@
<plugin
filename="ignition-gazebo-thermal-system"
name="ignition::gazebo::systems::Thermal">
<temperature>200.0</temperature>
</plugin>
</visual>
</link>
</model>

<model name="sphere">
<pose>0 1.5 0.5 0 0 0</pose>
<link name="sphere_link">
<inertial>
<inertia>
<ixx>3</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>3</iyy>
<iyz>0</iyz>
<izz>3</izz>
</inertia>
<mass>3.0</mass>
</inertial>
<collision name="sphere_collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="sphere_visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>0 0 1 1</specular>
</material>
<plugin
filename="ignition-gazebo-thermal-system"
name="ignition::gazebo::systems::Thermal">
<temperature>600.0</temperature>
</plugin>
</visual>
</link>
</model>

<model name="cylinder">
<pose>0 -1.5 0.5 0 0 0</pose>
<link name="cylinder_link">
<inertial>
<inertia>
<ixx>2</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2</iyy>
<iyz>0</iyz>
<izz>2</izz>
</inertia>
<mass>2.0</mass>
</inertial>
<collision name="cylinder_collision">
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1.0</length>
</cylinder>
</geometry>
</collision>

<visual name="cylinder_visual">
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1.0</length>
</cylinder>
</geometry>
<material>
<ambient>0 1 0 1</ambient>
<diffuse>0 1 0 1</diffuse>
<specular>0 1 0 1</specular>
</material>
<plugin
filename="ignition-gazebo-thermal-system"
name="ignition::gazebo::systems::Thermal">
<temperature>400.0</temperature>
<temperature>285.0</temperature>
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -323,5 +244,23 @@
<static>true</static>
</model>

<include>
<pose>1 0 0 0 0 1.570796</pose>
<name>rescue_randy</name>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy</uri>
</include>

<include>
<pose>2.25 .5 .1 0 0 1.570796</pose>
<name>phone</name>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black</uri>
</include>

<include>
<pose>2.25 -.5 .1 0 0 1.570796</pose>
<name>backpack</name>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack</uri>
</include>

</world>
</sdf>
99 changes: 99 additions & 0 deletions include/ignition/gazebo/components/TemperatureRange.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
/*
* 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.
*
*/
#ifndef IGNITION_GAZEBO_COMPONENTS_TEMPERATURERANGE_HH_
#define IGNITION_GAZEBO_COMPONENTS_TEMPERATURERANGE_HH_

#include <istream>
#include <ostream>

#include <ignition/math/Temperature.hh>

#include <ignition/gazebo/components/Factory.hh>
#include <ignition/gazebo/components/Component.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 Data structure to hold a temperature range, in kelvin
struct TemperatureRangeInfo
{
/// \brief The minimum temperature (kelvin)
math::Temperature min;
/// \brief The maximum temperature (kelvin)
math::Temperature max;

public: bool operator==(const TemperatureRangeInfo &_info) const
{
return (this->min == _info.min) &&
(this->max == _info.max);
}

public: bool operator!=(const TemperatureRangeInfo &_info) const
{
return !(*this == _info);
}
};
}

namespace serializers
{
/// \brief Serializer for components::TemperatureRangeInfo object
class TemperatureRangeInfoSerializer
{
/// \brief Serialization for components::TemperatureRangeInfo
/// \param[out] _out Output stream
/// \param[in] _info Object for the stream
/// \return The stream
public: static std::ostream &Serialize(std::ostream &_out,
const components::TemperatureRangeInfo &_info)
{
_out << _info.min << " " << _info.max;
return _out;
}

/// \brief Deserialization for components::TemperatureRangeInfo
/// \param[in] _in Input stream
/// \param[out] _info The object to populate
/// \return The stream
public: static std::istream &Deserialize(std::istream &_in,
components::TemperatureRangeInfo &_info)
{
_in >> _info.min >> _info.max;
return _in;
}
};
}

namespace components
{
/// \brief A component that stores a temperature range in kelvin
using TemperatureRange = Component<TemperatureRangeInfo,
class TemperatureRangeTag, serializers::TemperatureRangeInfoSerializer>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.TemperatureRange",
TemperatureRange)
}
}
}
}

#endif
60 changes: 50 additions & 10 deletions src/rendering/RenderUtil.cc
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,10 @@
*/

#include <map>
#include <string>
#include <tuple>
#include <unordered_map>
#include <variant>
#include <vector>

#include <sdf/Actor.hh>
Expand Down Expand Up @@ -58,7 +61,9 @@
#include "ignition/gazebo/components/Pose.hh"
#include "ignition/gazebo/components/RgbdCamera.hh"
#include "ignition/gazebo/components/Scene.hh"
#include "ignition/gazebo/components/SourceFilePath.hh"
#include "ignition/gazebo/components/Temperature.hh"
#include "ignition/gazebo/components/TemperatureRange.hh"
#include "ignition/gazebo/components/ThermalCamera.hh"
#include "ignition/gazebo/components/Transparency.hh"
#include "ignition/gazebo/components/Visibility.hh"
Expand Down Expand Up @@ -167,8 +172,19 @@ class ignition::gazebo::RenderUtilPrivate
public: std::map<Entity, std::map<std::string, math::Matrix4d>>
actorTransforms;

/// \brief A map of entity ids and temperature
public: std::map<Entity, float> entityTemp;
/// \brief A map of entity ids and temperature data.
/// The value of this map (tuple) represents either a single (uniform)
/// temperature, or a heat signature with a min/max temperature. If the string
/// in the tuple is empty, then this entity has a uniform temperature across
/// its surface, and this uniform temperature is stored in the first float of
/// the tuple (the second float and string are unused for uniform temperature
/// entities). If the string in the tuple is not empty, then the string
/// represents the entity's heat signature (a path to a heat signature texture
/// file), and the floats represent the min/max temperatures of the heat
/// signature, respectively.
///
/// All temperatures are in Kelvin.
public: std::map<Entity, std::tuple<float, float, std::string>> entityTemp;

/// \brief A map of entity ids and wire boxes
public: std::unordered_map<Entity, ignition::rendering::WireBoxPtr> wireBoxes;
Expand Down Expand Up @@ -596,7 +612,7 @@ void RenderUtil::Update()
}

// set visual temperature
for (auto &temp : entityTemp)
for (const auto &temp : entityTemp)
{
auto node = this->dataPtr->sceneManager.NodeById(temp.first);
if (!node)
Expand All @@ -607,7 +623,15 @@ void RenderUtil::Update()
if (!visual)
continue;

visual->SetUserData("temperature", temp.second);
const auto &heatSignature = std::get<2>(temp.second);
if (heatSignature.empty())
visual->SetUserData("temperature", std::get<0>(temp.second));
else
{
visual->SetUserData("minTemp", std::get<0>(temp.second));
visual->SetUserData("maxTemp", std::get<1>(temp.second));
visual->SetUserData("temperature", heatSignature);
}
}
}

Expand Down Expand Up @@ -724,13 +748,29 @@ void RenderUtilPrivate::CreateRenderingEntities(
visual.SetMaterial(material->Data());
}

// todo(anyone) make visual updates more generic without using extra
// variables like entityTemp just for storing one specific visual
// param?
auto temp = _ecm.Component<components::Temperature>(_entity);
if (temp)
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
{
this->entityTemp[_entity] = temp->Data().Kelvin();
// 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(
Expand Down
21 changes: 21 additions & 0 deletions src/systems/sensors/Sensors.cc
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,10 @@ class ignition::gazebo::systems::SensorsPrivate
/// sea level
public: double ambientTemperature = 288.15;

/// \brief Temperature gradient with respect to increasing altitude at sea
/// level in units of K/m.
public: double ambientTemperatureGradient = -0.0065;

/// \brief Keep track of cameras, in case we need to handle stereo cameras.
/// Key: Camera's parent scoped name
/// Value: Pointer to camera
Expand Down Expand Up @@ -386,6 +390,8 @@ void Sensors::Configure(const Entity &/*_id*/,
{
auto atmosphereSdf = atmosphere->Data();
this->dataPtr->ambientTemperature = atmosphereSdf.Temperature().Kelvin();
this->dataPtr->ambientTemperatureGradient =
atmosphereSdf.TemperatureGradient();
}

// Set render engine if specified from command line
Expand Down Expand Up @@ -564,6 +570,21 @@ std::string Sensors::CreateSensor(const Entity &_entity,
if (nullptr != thermalSensor)
{
thermalSensor->SetAmbientTemperature(this->dataPtr->ambientTemperature);

// temperature gradient is in kelvin per meter - typically change in
// temperature over change in altitude. However the implementation of
// thermal sensor in ign-sensors varies temperature for all objects in its
// view. So we will do an approximation based on camera view's vertical
// distance.
auto camSdf = _sdf.CameraSensor();
double farClip = camSdf->FarClip();
double angle = camSdf->HorizontalFov().Radian();
double aspect = camSdf->ImageWidth() / camSdf->ImageHeight();
double vfov = 2.0 * atan(tan(angle / 2.0) / aspect);
double height = tan(vfov / 2.0) * farClip * 2.0;
double tempRange =
std::fabs(this->dataPtr->ambientTemperatureGradient * height);
thermalSensor->SetAmbientTemperatureRange(tempRange);
}

return sensor->Name();
Expand Down
Loading

0 comments on commit b4e152d

Please sign in to comment.