diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index c8498d3cc2..3e2230f099 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -1,6 +1,6 @@ @@ -121,7 +121,14 @@ - 310 + 300 + + 0.1 @@ -157,7 +164,7 @@ - 0 0 0.5 0 0 0 + -1 1 0.5 0 0 0 @@ -192,93 +199,7 @@ - 200.0 - - - - - - - 0 1.5 0.5 0 0 0 - - - - 3 - 0 - 0 - 3 - 0 - 3 - - 3.0 - - - - - 0.5 - - - - - - - - 0.5 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - 600.0 - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 2 - 0 - 0 - 2 - 0 - 2 - - 2.0 - - - - - 0.5 - 1.0 - - - - - - - - 0.5 - 1.0 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - 400.0 + 285.0 @@ -323,5 +244,23 @@ true + + 1 0 0 0 0 1.570796 + rescue_randy + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + + + + 2.25 .5 .1 0 0 1.570796 + phone + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black + + + + 2.25 -.5 .1 0 0 1.570796 + backpack + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + + diff --git a/include/ignition/gazebo/components/TemperatureRange.hh b/include/ignition/gazebo/components/TemperatureRange.hh new file mode 100644 index 0000000000..99e4a26151 --- /dev/null +++ b/include/ignition/gazebo/components/TemperatureRange.hh @@ -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 +#include + +#include + +#include +#include +#include + +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; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.TemperatureRange", + TemperatureRange) +} +} +} +} + +#endif diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index acf90e1347..5243a7a31b 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -16,7 +16,10 @@ */ #include +#include +#include #include +#include #include #include @@ -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" @@ -167,8 +172,19 @@ class ignition::gazebo::RenderUtilPrivate public: std::map> actorTransforms; - /// \brief A map of entity ids and temperature - public: std::map 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> entityTemp; /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -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) @@ -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); + } } } @@ -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(_entity); - if (temp) + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = + std::make_tuple( + 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(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) + { + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); + } } this->newVisuals.push_back( diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index d973d94a60..20a929854d 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -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 @@ -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 @@ -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(); diff --git a/src/systems/thermal/Thermal.cc b/src/systems/thermal/Thermal.cc index 2a0f346797..28bd118931 100644 --- a/src/systems/thermal/Thermal.cc +++ b/src/systems/thermal/Thermal.cc @@ -15,16 +15,23 @@ * */ -#include +#include "Thermal.hh" + +#include +#include + +#include +#include #include -#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Atmosphere.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" -#include "Thermal.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -48,14 +55,79 @@ void Thermal::Configure(const Entity &_entity, gazebo::EntityComponentManager &_ecm, gazebo::EventManager & /*_eventMgr*/) { - if (!_sdf->HasElement("temperature")) + const std::string temperatureTag = "temperature"; + const std::string heatSignatureTag = "heat_signature"; + const std::string minTempTag = "min_temp"; + const std::string maxTempTag = "max_temp"; + + if (_sdf->HasElement(temperatureTag) && _sdf->HasElement(heatSignatureTag)) + { + ignwarn << "Both <" << temperatureTag << "> and <" << heatSignatureTag + << "> were specified, but the thermal system only uses one. " + << "<" << heatSignatureTag << "> will be used.\n"; + } + + if (_sdf->HasElement(heatSignatureTag)) + { + std::string heatSignature = _sdf->Get(heatSignatureTag); + auto modelEntity = topLevelModel(_entity, _ecm); + auto modelPath = + _ecm.ComponentData(modelEntity); + auto path = common::findFile(asFullPath(heatSignature, modelPath.value())); + + // make sure the specified heat signature can be found + if (path.empty()) + { + ignerr << "Failed to load thermal system. Heat signature [" + << heatSignature << "] could not be found\n"; + return; + } + _ecm.CreateComponent(_entity, components::SourceFilePath(path)); + + // see if the user defined a custom temperature range + // (default to ambient temperature if possible) + double min = 250.0; + double max = 300.0; + if (auto worldEntity = _ecm.EntityByComponents(components::World())) + { + if (auto atmosphere = + _ecm.Component(worldEntity)) + { + auto atmosphericTemp = atmosphere->Data().Temperature().Kelvin(); + min = atmosphericTemp; + max = atmosphericTemp; + } + } + if (_sdf->HasElement(minTempTag)) + min = _sdf->Get(minTempTag); + if (_sdf->HasElement(maxTempTag)) + max = _sdf->Get(maxTempTag); + // make sure that min is actually less than max + if (min > max) + { + auto temporary = max; + max = min; + min = temporary; + } + igndbg << "Thermal plugin, heat signature: using a minimum temperature of " + << min << " kelvin, and a max temperature of " << max << " kelvin.\n"; + + components::TemperatureRangeInfo rangeInfo; + rangeInfo.min = min; + rangeInfo.max = max; + _ecm.CreateComponent(_entity, components::TemperatureRange(rangeInfo)); + } + else if (_sdf->HasElement(temperatureTag)) + { + double temperature = _sdf->Get(temperatureTag); + _ecm.CreateComponent(_entity, components::Temperature(temperature)); + } + else { - ignerr << "Fail to load thermal system: is not specified" - << std::endl; - return; + ignerr << "Failed to load thermal system. " + << "Neither <" << temperatureTag << "> or <" << heatSignatureTag + << "> were specified.\n"; } - double temperature = _sdf->Get("temperature"); - _ecm.CreateComponent(_entity, components::Temperature(temperature)); } diff --git a/test/integration/components.cc b/test/integration/components.cc index 57bc96a2ba..fed087ef78 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -71,6 +71,7 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -1376,6 +1377,45 @@ TEST_F(ComponentsTest, Static) EXPECT_FALSE(comp11 != comp12); } +///////////////////////////////////////////////// +TEST_F(ComponentsTest, TemperatureRange) +{ + // TODO(adlarkin) make sure min can't be >= max? + components::TemperatureRangeInfo range1; + range1.min = math::Temperature(125.0); + range1.max = math::Temperature(300.0); + + components::TemperatureRangeInfo range2; + range2.min = math::Temperature(140.0); + range2.max = math::Temperature(200.0); + + components::TemperatureRangeInfo range3; + range3.min = math::Temperature(125.0); + range3.max = math::Temperature(300.0); + + // Create components + auto comp1 = components::TemperatureRange(range1); + auto comp2 = components::TemperatureRange(range2); + auto comp3 = components::TemperatureRange(range3); + + // Equality operators + EXPECT_EQ(comp1, comp3); + EXPECT_NE(comp1, comp2); + EXPECT_NE(comp2, comp3); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("125 300", ostr.str()); + + std::istringstream istr(ostr.str()); + components::TemperatureRange comp4; + comp4.Deserialize(istr); + EXPECT_EQ(comp1, comp4); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, ThreadPitch) { diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 4fffa35ee5..211f06b28d 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -21,7 +21,9 @@ #include #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -29,8 +31,6 @@ #include "../helpers/Relay.hh" -#define tol 10e-4 - using namespace ignition; using namespace gazebo; @@ -62,6 +62,9 @@ TEST_F(ThermalTest, TemperatureComponent) test::Relay testSystem; std::map entityTemp; + std::map + entityTempRange; + std::map heatSignatures; testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { @@ -76,20 +79,93 @@ TEST_F(ThermalTest, TemperatureComponent) // verify temperature data belongs to a visual EXPECT_NE(nullptr, _ecm.Component(_id)); + return true; + }); + + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::TemperatureRange *_tempRange, + const components::SourceFilePath *_heatSigURI, + const components::Name *_name) -> bool + { + // store temperature range data + entityTempRange[_name->Data()] = _tempRange->Data(); + + // store heat signature URI data + heatSignatures[_name->Data()] = _heatSigURI->Data(); + + // verify temperature range data belongs to a visual + EXPECT_NE(nullptr, _ecm.Component(_id)); + return true; }); }); server.AddSystem(testSystem.systemPtr); - // verify nothing in map at beginning + // verify nothing in the maps at beginning EXPECT_TRUE(entityTemp.empty()); + EXPECT_TRUE(entityTempRange.empty()); + EXPECT_TRUE(heatSignatures.empty()); // Run server server.Run(true, 1, false); + const std::string sphereVisual = "sphere_visual"; + const std::string cylinderVisual = "cylinder_visual"; + const std::string visual = "visual"; + const std::string heatSignatureCylinderVisual = + "heat_signature_cylinder_visual"; + const std::string heatSignatureSphereVisual = + "heat_signature_sphere_visual"; + const std::string heatSignatureSphereVisual2 = + "heat_signature_sphere_visual_2"; + const std::string heatSignatureTestResource = "duck.png"; + // verify temperature components are created and the values are correct - EXPECT_EQ(3u, entityTemp.size()); - EXPECT_DOUBLE_EQ(200.0, entityTemp["box_visual"].Kelvin()); - EXPECT_DOUBLE_EQ(600.0, entityTemp["sphere_visual"].Kelvin()); - EXPECT_DOUBLE_EQ(400.0, entityTemp["cylinder_visual"].Kelvin()); + 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()); + EXPECT_DOUBLE_EQ(400.0, entityTemp[cylinderVisual].Kelvin()); + + EXPECT_EQ(4u, entityTempRange.size()); + ASSERT_TRUE(entityTempRange.find(visual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureCylinderVisual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureSphereVisual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureSphereVisual2) != entityTempRange.end()); + EXPECT_DOUBLE_EQ(310.0, entityTempRange[visual].min.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, entityTempRange[visual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureCylinderVisual].min.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureCylinderVisual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureSphereVisual].min.Kelvin()); + EXPECT_DOUBLE_EQ(500.0, + entityTempRange[heatSignatureSphereVisual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureSphereVisual2].min.Kelvin()); + EXPECT_DOUBLE_EQ(400.0, + entityTempRange[heatSignatureSphereVisual2].max.Kelvin()); + + EXPECT_EQ(4u, heatSignatures.size()); + ASSERT_TRUE(heatSignatures.find(visual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureCylinderVisual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureSphereVisual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureSphereVisual2) != heatSignatures.end()); + EXPECT_TRUE(heatSignatures[visual].find( + "RescueRandy_Thermal.png") != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureCylinderVisual].find( + heatSignatureTestResource) != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual].find( + heatSignatureTestResource) != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual2].find( + heatSignatureTestResource) != std::string::npos); } diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf index 223b04ba51..16a8436c0d 100644 --- a/test/worlds/thermal.sdf +++ b/test/worlds/thermal.sdf @@ -74,93 +74,100 @@ - - 0 0 0.5 0 0 0 - + + 0 1.5 0.5 0 0 0 + - 1 + 3 0 0 - 1 + 3 0 - 1 + 3 - 1.0 + 3.0 - + - - 1 1 1 - + + 0.5 + - + - - 1 1 1 - + + 0.5 + - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 - 200.0 + 600.0 - - 0 1.5 0.5 0 0 0 - + + 0 -1.5 0.5 0 0 0 + - 3 + 2 0 0 - 3 + 2 0 - 3 + 2 - 3.0 + 2.0 - + - + 0.5 - + 1.0 + - + - + 0.5 - + 1.0 + - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 - 600.0 + 400.0 - - 0 -1.5 0.5 0 0 0 - + + + -2 -1.5 0.5 0 0 0 + 2 @@ -172,7 +179,7 @@ 2.0 - + 0.5 @@ -181,7 +188,7 @@ - + 0.5 @@ -196,10 +203,110 @@ + ../media/duck.png 400.0 + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + ../media/duck.png + 500.0 + + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + ../media/duck.png + 400.0 + + + + + + + + 0 0 0 0 0 1.570796 + rescue_randy + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + +