Skip to content

Commit

Permalink
Add test for thermal object temperatures below 0 kelvin (#621)
Browse files Browse the repository at this point in the history
Signed-off-by: Ashton Larkin <[email protected]>
  • Loading branch information
adlarkin committed Mar 19, 2021
1 parent f9a5ece commit 167f086
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 4 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ set(IGN_SENSORS_VER ${ignition-sensors4_VERSION_MAJOR})

#--------------------------------------
# Find ignition-rendering
ign_find_package(ignition-rendering4 REQUIRED VERSION 4.1.0)
ign_find_package(ignition-rendering4 REQUIRED VERSION 4.7.0)
set(IGN_RENDERING_VER ${ignition-rendering4_VERSION_MAJOR})

#--------------------------------------
Expand Down
12 changes: 11 additions & 1 deletion test/integration/thermal_sensor_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -171,13 +171,18 @@ TEST_F(ThermalSensorTest,
// verify temperature of heat source
const std::string sphereVisual = "sphere_visual";
const std::string cylinderVisual = "cylinder_visual";
EXPECT_EQ(2u, entityTemp.size());
const std::string boxVisual = "box_visual";
EXPECT_EQ(3u, entityTemp.size());
ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end());
ASSERT_TRUE(entityTemp.find(cylinderVisual) != entityTemp.end());
ASSERT_TRUE(entityTemp.find(boxVisual) != 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());
// the user specified temp is less than the min value of the camera - this
// value should be clamped
EXPECT_DOUBLE_EQ(-10.0, entityTemp[boxVisual].Kelvin());

// Run server
server.Run(true, 10, false);
Expand All @@ -201,16 +206,21 @@ TEST_F(ThermalSensorTest,
std::lock_guard<std::mutex> lock(g_mutex);
unsigned int leftIdx = height * 0.5 * width;
unsigned int rightIdx = leftIdx + width-1;
unsigned int middleIdx = leftIdx + (0.5 * width);
unsigned int defaultResolution = 3u;
unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution;
unsigned int sphereTemp = g_image[rightIdx] * defaultResolution;
unsigned int boxTemp = g_image[middleIdx] * 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);
// the box resolution should be clamped to the camera's default
// minimum temperature (0)
EXPECT_EQ(0u, boxTemp);
}

g_imageMsgs.clear();
Expand Down
36 changes: 34 additions & 2 deletions test/worlds/thermal_invalid.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@

<model name="sphere">
<static>true</static>`
<pose>0 0.5 0.5 0 0 0</pose>
<pose>0 0.85 0.5 0 0 0</pose>
<link name="sphere_link">
<collision name="sphere_collision">
<geometry>
Expand Down Expand Up @@ -70,7 +70,7 @@

<model name="cylinder">
<static>true</static>`
<pose>0 -0.5 0.5 0 0 0</pose>
<pose>0 -0.85 0.5 0 0 0</pose>
<link name="cylinder_link">
<collision name="cylinder_collision">
<geometry>
Expand Down Expand Up @@ -102,6 +102,38 @@
</link>
</model>

<model name="box">
<static>true</static>`
<pose>-1 0.0 0.5 0 0 0</pose>
<link name="box_link">
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
<plugin
filename="ignition-gazebo-thermal-system"
name="ignition::gazebo::systems::Thermal">
<temperature>-10.0</temperature>
</plugin>
</visual>
</link>
</model>

<model name="thermal_camera_invalid">
<pose>1.0 0 0.5 0.0 0.0 3.14</pose>
<link name="link">
Expand Down

0 comments on commit 167f086

Please sign in to comment.