diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index dc5ea86166..5bb2d77116 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -722,18 +722,23 @@ void RenderUtil::Update() this->dataPtr->sceneManager.CreateLight( std::get<0>(light), std::get<1>(light), std::get<2>(light)); - // create a new id for the light visual - auto attempts = 100000u; - for (auto i = 0u; i < attempts; ++i) + // TODO(anyone) This needs to be updated for when sensors and GUI use + // the same scene + // create a new id for the light visual, if we're not loading sensors + if (!this->dataPtr->enableSensors) { - Entity id = std::numeric_limits::min() + i; - if (!this->dataPtr->sceneManager.HasEntity(id)) + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) { - rendering::VisualPtr lightVisual = - this->dataPtr->sceneManager.CreateLightVisual( - id, std::get<1>(light), std::get<0>(light)); - this->dataPtr->matchLightWithVisuals[std::get<0>(light)] = id; - break; + Entity id = std::numeric_limits::min() + i; + if (!this->dataPtr->sceneManager.HasEntity(id)) + { + rendering::VisualPtr lightVisual = + this->dataPtr->sceneManager.CreateLightVisual( + id, std::get<1>(light), std::get<0>(light)); + this->dataPtr->matchLightWithVisuals[std::get<0>(light)] = id; + break; + } } } } diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc index 5d076fa9f7..cea5854ef1 100644 --- a/test/integration/thermal_sensor_system.cc +++ b/test/integration/thermal_sensor_system.cc @@ -63,6 +63,11 @@ void thermalCb(const msgs::Image &_msg) std::lock_guard g_lock(g_mutex); g_imageMsgs.push_back(_msg); + EXPECT_EQ(320u, _msg.width()); + EXPECT_EQ(240u, _msg.height()); + EXPECT_EQ(320u, _msg.step()); + EXPECT_EQ(msgs::PixelFormatType::L_INT8, _msg.pixel_format_type()); + unsigned int width = _msg.width(); unsigned int height = _msg.height(); unsigned int size = width * height * sizeof(unsigned char); @@ -174,9 +179,9 @@ TEST_F(ThermalSensorTest, const std::string cylinderVisual = "cylinder_visual"; 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()); + ASSERT_NE(entityTemp.find(sphereVisual), entityTemp.end()); + ASSERT_NE(entityTemp.find(cylinderVisual), entityTemp.end()); + ASSERT_NE(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 @@ -186,20 +191,19 @@ TEST_F(ThermalSensorTest, EXPECT_DOUBLE_EQ(-10.0, entityTemp[boxVisual].Kelvin()); // Run server - server.Run(true, 35, false); + server.Run(true, 10, false); // wait for image bool received = false; - for (unsigned int i = 0; i < 30; ++i) + int sleep{0}; + int maxSleep{40}; + for (; sleep < maxSleep && !received; ++sleep) { - { - std::lock_guard lock(g_mutex); - received = !g_imageMsgs.empty(); - } - if (received) - break; std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(g_mutex); + received = !g_imageMsgs.empty(); } + EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(received); ASSERT_NE(nullptr, g_image);