diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index cf5c92b3f4..e21215537a 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -104,6 +104,18 @@ thermal_camera false + + + Thermal camera 8 Bit + floating + 350 + 315 + 500 + + thermal_camera_8bit/image + false + + @@ -230,6 +242,7 @@ 320 240 + L16 0.1 @@ -245,6 +258,55 @@ true + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + + + + true + + + 1 0 0 0 0 1.570796 rescue_randy diff --git a/include/ignition/gazebo/components/Temperature.hh b/include/ignition/gazebo/components/Temperature.hh index 228e208518..cfd8df5327 100644 --- a/include/ignition/gazebo/components/Temperature.hh +++ b/include/ignition/gazebo/components/Temperature.hh @@ -35,6 +35,12 @@ namespace components using Temperature = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Temperature", Temperature) + + /// \brief A component that stores temperature linear resolution in Kelvin + using TemperatureLinearResolution = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.TemperatureLinearResolution", + TemperatureLinearResolution) } } } diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 600744db97..5d64553867 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/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); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 563078231a..c1c507713b 100644 --- a/src/rendering/RenderUtil.cc +++ b/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 viewingCollisions; + + /// \brief A map of entity id to thermal camera sensor configuration + /// properties. The elements in the tuple are: + /// + public:std::unordered_map> 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 lock(this->dataPtr->updateMutex); + // Update thermal cameras + _ecm.Each( + [&](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::max(); + range.max = 0; + + // resolution + auto resolutionComp = + _ecm.Component(_entity); + if (resolutionComp != nullptr) + { + resolution = resolutionComp->Data(); + _ecm.RemoveComponent( + _entity); + } + + // min / max temp + auto tempRangeComp = + _ecm.Component(_entity); + if (tempRangeComp != nullptr) + { + range = tempRangeComp->Data(); + _ecm.RemoveComponent(_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( + 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(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = + std::make_tuple( + 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(_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( std::make_tuple(_entity, visual, _parent->Data())); return true; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 20a929854d..93d14fb354 100644 --- a/src/systems/sensors/Sensors.cc +++ b/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 ) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index b295159cf8..3ee5807268 100644 --- a/src/systems/sensors/Sensors.hh +++ b/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; diff --git a/src/systems/thermal/CMakeLists.txt b/src/systems/thermal/CMakeLists.txt index ff3e7aa265..c6d6dfc3ba 100644 --- a/src/systems/thermal/CMakeLists.txt +++ b/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} +) diff --git a/src/systems/thermal/ThermalSensor.cc b/src/systems/thermal/ThermalSensor.cc new file mode 100644 index 0000000000..72e5f60a45 --- /dev/null +++ b/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 +#include + +#include +#include + +#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()) +{ +} + +////////////////////////////////////////////////// +ThermalSensor::~ThermalSensor() = default; + +////////////////////////////////////////////////// +void ThermalSensor::Configure(const Entity &_entity, + const std::shared_ptr &_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(_entity); + if (sensorComp == nullptr) + { + ignerr << "The thermal sensor system can only be used to configure " + << "parameters of thermal camera sensor " << std::endl; + return; + } + + if (_sdf->HasElement(resolutionTag)) + { + double resolution = _sdf->Get(resolutionTag); + _ecm.CreateComponent(_entity, + components::TemperatureLinearResolution(resolution)); + } + + if (_sdf->HasElement(minTempTag) || _sdf->HasElement(maxTempTag)) + { + double min = 0.0; + double max = std::numeric_limits::max(); + min = _sdf->Get(minTempTag, min).first; + max = _sdf->Get(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") diff --git a/src/systems/thermal/ThermalSensor.hh b/src/systems/thermal/ThermalSensor.hh new file mode 100644 index 0000000000..ed2fb431f2 --- /dev/null +++ b/src/systems/thermal/ThermalSensor.hh @@ -0,0 +1,60 @@ +/* + * 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_SYSTEMS_THERMALSENSOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_THERMALSENSOR_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ThermalSensorPrivate; + + /// \brief A thermal sensor plugin for configuring thermal sensor properties + class IGNITION_GAZEBO_VISIBLE ThermalSensor: + public System, + public ISystemConfigure + { + /// \brief Constructor + public: explicit ThermalSensor(); + + /// \brief Destructor + public: ~ThermalSensor() override; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index a2e4eb484f..5e2233fbd0 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -42,7 +42,6 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc - thermal_system.cc touch_plugin.cc triggered_publisher.cc user_commands.cc @@ -60,6 +59,8 @@ set(tests_needing_display gpu_lidar.cc rgbd_camera.cc sensors_system.cc + thermal_system.cc + thermal_sensor_system.cc ) # Add systems that need a valid display here. diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc new file mode 100644 index 0000000000..472a8a60ff --- /dev/null +++ b/test/integration/thermal_sensor_system.cc @@ -0,0 +1,219 @@ +/* + * 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 + +#include +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test Thermal system +class ThermalSensorTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + + +std::mutex g_mutex; +std::vector g_imageMsgs; +unsigned char *g_image = nullptr; + +///////////////////////////////////////////////// +void thermalCb(const msgs::Image &_msg) +{ + std::lock_guard g_lock(g_mutex); + g_imageMsgs.push_back(_msg); + + unsigned int width = _msg.width(); + unsigned int height = _msg.height(); + unsigned int size = width * height * sizeof(unsigned char); + if (!g_image) + { + g_image = new unsigned char[size]; + } + memcpy(g_image, _msg.data().c_str(), size); +} + +///////////////////////////////////////////////// +TEST_F(ThermalSensorTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystemInvalidConfig)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal_invalid.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::map entityTemp; + std::string name; + sdf::Sensor sensorSdf; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::Temperature *_temp, + const components::Name *_name) -> bool + { + // store temperature data + entityTemp[_name->Data()] = _temp->Data(); + + // verify temperature data belongs to a visual + EXPECT_NE(nullptr, _ecm.Component(_id)); + + return true; + }); + }); + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + if (resolutionComp) + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + if (temperatureRangeComp) + { + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // subscribe to thermal topic + transport::Node node; + node.Subscribe("/thermal_camera_invalid/image", &thermalCb); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + unsigned int width = 320u; + unsigned int height = 240u; + EXPECT_EQ("thermal_camera_invalid", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(width, cameraSdf->ImageWidth()); + EXPECT_EQ(height, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + // the resolution, min and max are invalid range values. Ign-gazebo should + // print out warnings and use default values + EXPECT_DOUBLE_EQ(0.0, resolution); + EXPECT_DOUBLE_EQ(999.0, minTemp); + EXPECT_DOUBLE_EQ(-590.0, maxTemp); + + // verify temperature of heat source + const std::string sphereVisual = "sphere_visual"; + const std::string cylinderVisual = "cylinder_visual"; + 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()); + // the user specified temp is larger than the max value representable by an + // 8 bit 3 degree resolution camera - this value should be clamped + EXPECT_DOUBLE_EQ(800.0, entityTemp[cylinderVisual].Kelvin()); + + // Run server + server.Run(true, 10, false); + + // wait for image + bool received = false; + for (unsigned int i = 0; i < 20; ++i) + { + { + std::lock_guard lock(g_mutex); + received = !g_imageMsgs.empty(); + } + if (received) + break; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); + + // check temperature in actual image output + { + std::lock_guard lock(g_mutex); + unsigned int leftIdx = height * 0.5 * width; + unsigned int rightIdx = leftIdx + width-1; + unsigned int defaultResolution = 3u; + unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution; + unsigned int sphereTemp = g_image[rightIdx] * defaultResolution; + // default resolution, min, max values used so we should get correct + // temperature value + EXPECT_EQ(600u, sphereTemp); + // 8 bit 3 degree resolution camera - this value should be clamped + // in the image output to: + // 2^(bitDepth) - 1 * resolution = 2^8 - 1 * 3 = 765 + EXPECT_EQ(765u, cylinderTemp); + } + + g_imageMsgs.clear(); + delete [] g_image; + g_image = nullptr; +} diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 211f06b28d..25b8a98bb0 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -16,14 +16,21 @@ */ #include + +#include +#include + #include +#include #include #include +#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/ThermalCamera.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -47,7 +54,7 @@ class ThermalTest : public ::testing::Test }; ///////////////////////////////////////////////// -TEST_F(ThermalTest, TemperatureComponent) +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) { // Start server ServerConfig serverConfig; @@ -169,3 +176,70 @@ TEST_F(ThermalTest, TemperatureComponent) EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual2].find( heatSignatureTestResource) != std::string::npos); } + +///////////////////////////////////////////////// +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::string name; + sdf::Sensor sensorSdf; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + EXPECT_NE(nullptr, resolutionComp); + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + EXPECT_NE(nullptr, temperatureRangeComp); + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + return true; + }); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + EXPECT_EQ("thermal_camera_8bit", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(320u, cameraSdf->ImageWidth()); + EXPECT_EQ(240u, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + EXPECT_DOUBLE_EQ(3.0, resolution); + EXPECT_DOUBLE_EQ(253.15, minTemp); + EXPECT_DOUBLE_EQ(673.15, maxTemp); +} diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf index 73cc58c228..5274b749b0 100644 --- a/test/worlds/thermal.sdf +++ b/test/worlds/thermal.sdf @@ -309,5 +309,51 @@ https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + + + true + diff --git a/test/worlds/thermal_invalid.sdf b/test/worlds/thermal_invalid.sdf new file mode 100644 index 0000000000..89c621b9c9 --- /dev/null +++ b/test/worlds/thermal_invalid.sdf @@ -0,0 +1,152 @@ + + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + 310 + + + + true` + 0 0.5 0.5 0 0 0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + 600.0 + + + + + + + true` + 0 -0.5 0.5 0 0 0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + 800.0 + + + + + + + 1.0 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_invalid/image + + 999 + -590 + 0.0 + + + + true + + +