diff --git a/src/Sensor.cc b/src/Sensor.cc index 90b8b95a..2d28e545 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -45,7 +45,7 @@ class ignition::sensors::SensorPrivate /// value from SDF, and zero is allowed only when zero is also /// in SDF. /// \return True if a valid topic was set. - public: void SetRate(const ignition::msgs::Double& _rate); + public: void SetRate(const ignition::msgs::Double &_rate); /// \brief id given to sensor when constructed public: SensorId id; @@ -234,7 +234,7 @@ bool SensorPrivate::SetTopic(const std::string &_topic) } ////////////////////////////////////////////////// -void SensorPrivate::SetRate(const ignition::msgs::Double& _rate) +void SensorPrivate::SetRate(const ignition::msgs::Double &_rate) { auto rate = _rate.data(); if (rate < 0.0) diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index e6573691..7dac341f 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -78,6 +78,9 @@ class ignition::sensors::ThermalCameraSensorPrivate /// \brief Thermal data buffer. public: uint16_t *thermalBuffer = nullptr; + /// \brief Thermal data buffer 8 bit. + public: unsigned char *thermalBuffer8Bit = nullptr; + /// \brief Thermal data buffer used when saving image. public: unsigned char *imgThermalBuffer = nullptr; @@ -158,6 +161,9 @@ ThermalCameraSensor::~ThermalCameraSensor() if (this->dataPtr->thermalBuffer) delete [] this->dataPtr->thermalBuffer; + if (this->dataPtr->thermalBuffer8Bit) + delete[] this->dataPtr->thermalBuffer8Bit; + if (this->dataPtr->imgThermalBuffer) delete[] this->dataPtr->imgThermalBuffer; } @@ -245,6 +251,8 @@ bool ThermalCameraSensor::CreateCamera() int width = cameraSdf->ImageWidth(); int height = cameraSdf->ImageHeight(); + sdf::PixelFormatType pixelFormat = cameraSdf->PixelFormat(); + double farPlane = cameraSdf->FarClip(); double nearPlane = cameraSdf->NearClip(); @@ -254,6 +262,30 @@ bool ThermalCameraSensor::CreateCamera() this->Name()); this->dataPtr->thermalCamera->SetImageWidth(width); this->dataPtr->thermalCamera->SetImageHeight(height); + switch (pixelFormat) + { + case sdf::PixelFormatType::L_INT8: + { + this->dataPtr->thermalCamera->SetImageFormat(rendering::PF_L8); + // sanity check for resolution. The default resolution 10mK is too high + // for 8 bit cameras since it can only capture a temperature range of + // 0 to 2.55 degrees Kelvin ((2^8-1) * 0.01 = 2.55). + if (this->dataPtr->resolution < 1.0) + { + ignwarn << "8 bit thermal camera image format selected. " + << "The temperature linear resolution needs to be higher " + << "than 1.0. Defaulting to 3.0, output range = [0, 255*3] K" + << std::endl; + this->dataPtr->resolution = 3.0; + } + break; + } + // default to 16 bit if format is not recognized + case sdf::PixelFormatType::L_INT16: + default: + this->dataPtr->thermalCamera->SetImageFormat(rendering::PF_L16); + break; + } this->dataPtr->thermalCamera->SetNearClipPlane(nearPlane); this->dataPtr->thermalCamera->SetFarClipPlane(farPlane); this->dataPtr->thermalCamera->SetVisibilityMask( @@ -406,14 +438,23 @@ bool ThermalCameraSensor::Update( unsigned int width = this->dataPtr->thermalCamera->ImageWidth(); unsigned int height = this->dataPtr->thermalCamera->ImageHeight(); + auto commonFormat = common::Image::L_INT16; auto msgsFormat = msgs::PixelFormatType::L_INT16; + auto renderingFormat = rendering::PF_L16; + + if (this->dataPtr->thermalCamera->ImageFormat() == rendering::PF_L8) + { + commonFormat = common::Image::L_INT8; + msgsFormat = msgs::PixelFormatType::L_INT8; + renderingFormat = rendering::PF_L8; + } // create message this->dataPtr->thermalMsg.set_width(width); this->dataPtr->thermalMsg.set_height(height); this->dataPtr->thermalMsg.set_step( - width * rendering::PixelUtil::BytesPerPixel(rendering::PF_L16)); + width * rendering::PixelUtil::BytesPerPixel(renderingFormat)); this->dataPtr->thermalMsg.set_pixel_format_type(msgsFormat); auto stamp = this->dataPtr->thermalMsg.mutable_header()->mutable_stamp(); *stamp = msgs::Convert(_now); @@ -422,9 +463,29 @@ bool ThermalCameraSensor::Update( frame->add_value(this->Name()); std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer, - rendering::PixelUtil::MemorySize(rendering::PF_L16, - width, height)); + + // \todo(anyone) once ign-rendering supports an image event with unsigned char + // data type, we can remove this check that copies uint16_t data to char array + if (this->dataPtr->thermalCamera->ImageFormat() == rendering::PF_L8) + { + unsigned int len = width * height; + if (!this->dataPtr->thermalBuffer8Bit) + this->dataPtr->thermalBuffer8Bit = new unsigned char[len]; + for (unsigned int i = 0; i < len; ++i) + { + this->dataPtr->thermalBuffer8Bit[i] = + static_cast(this->dataPtr->thermalBuffer[i]); + } + this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer8Bit, + rendering::PixelUtil::MemorySize(renderingFormat, + width, height)); + } + else + { + this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer, + rendering::PixelUtil::MemorySize(renderingFormat, + width, height)); + } // publish the camera info message this->PublishInfo(_now); diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera_plugin.cc index fc2463d9..59c0843b 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera_plugin.cc @@ -55,6 +55,7 @@ std::mutex g_mutex; unsigned int g_thermalCounter = 0; uint16_t *g_thermalBuffer = nullptr; +unsigned char *g_thermalBuffer8Bit = nullptr; std::mutex g_infoMutex; unsigned int g_infoCounter = 0; @@ -80,11 +81,26 @@ void OnImage(const ignition::msgs::Image &_msg) g_mutex.unlock(); } +void OnImage8Bit(const ignition::msgs::Image &_msg) +{ + g_mutex.lock(); + unsigned int thermalSamples = _msg.width() * _msg.height(); + unsigned int thermalBufferSize = thermalSamples * sizeof(unsigned char); + if (!g_thermalBuffer8Bit) + g_thermalBuffer8Bit = new unsigned char[thermalSamples]; + memcpy(g_thermalBuffer8Bit, _msg.data().c_str(), thermalBufferSize); + g_thermalCounter++; + g_mutex.unlock(); +} + class ThermalCameraSensorTest: public testing::Test, public testing::WithParamInterface { // Create a Camera sensor from a SDF and gets a image message public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); + + // Create a thermal camera sensor from a SDF with 8 bit image format + public: void Images8BitWithBuiltinSDF(const std::string &_renderEngine); }; void ThermalCameraSensorTest::ImagesWithBuiltinSDF( @@ -330,6 +346,8 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_mutex.unlock(); + delete [] g_thermalBuffer; + g_thermalBuffer = nullptr; // Clean up engine->DestroyScene(scene); @@ -342,6 +360,266 @@ TEST_P(ThermalCameraSensorTest, ImagesWithBuiltinSDF) ImagesWithBuiltinSDF(GetParam()); } +////////////////////////////////////////////////// +void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( + const std::string &_renderEngine) +{ + // get the darn test data + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "integration", "thermal_camera_sensor_8bit_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + ASSERT_TRUE(sensorPtr->HasElement("camera")); + auto cameraPtr = sensorPtr->GetElement("camera"); + ASSERT_TRUE(cameraPtr->HasElement("image")); + auto imagePtr = cameraPtr->GetElement("image"); + + std::string format = imagePtr->Get("format"); + EXPECT_EQ("L8", format); + + ASSERT_TRUE(cameraPtr->HasElement("clip")); + auto clipPtr = cameraPtr->GetElement("clip"); + int imgWidth = imagePtr->Get("width"); + int imgHeight = imagePtr->Get("height"); + double far_ = clipPtr->Get("far"); + double near_ = clipPtr->Get("near"); + + double unitBoxSize = 1.0; + ignition::math::Vector3d boxPosition(3.0, 0.0, 0.0); + + // If ogre2 is not the engine, don't run the test + if ((_renderEngine.compare("ogre2") != 0)) + { + igndbg << "Engine '" << _renderEngine + << "' doesn't support 8 bit thermal cameras" << std::endl; + return; + } + + // Setup ign-rendering with an empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + + // Create an scene with a box in it + scene->SetAmbientLight(0.3, 0.3, 0.3); + ignition::rendering::VisualPtr root = scene->RootVisual(); + + // create box visual + ignition::rendering::VisualPtr box = scene->CreateVisual(); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(boxPosition); + box->SetLocalRotation(0, 0, 0); + box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); + + // set box temperature + float boxTemp = 310.0; + box->SetUserData("temperature", boxTemp); + + root->AddChild(box); + + ignition::sensors::Manager mgr; + + ignition::sensors::ThermalCameraSensor *thermalSensor = + mgr.CreateSensor(sensorPtr); + ASSERT_NE(thermalSensor, nullptr); + + float ambientTemp = 296.0f; + float ambientTempRange = 4.0f; + float linearResolution = 3.0f; + thermalSensor->SetAmbientTemperature(ambientTemp); + thermalSensor->SetAmbientTemperatureRange(ambientTempRange); + thermalSensor->SetLinearResolution(linearResolution); + thermalSensor->SetScene(scene); + + EXPECT_EQ(thermalSensor->ImageWidth(), static_cast(imgWidth)); + EXPECT_EQ(thermalSensor->ImageHeight(), static_cast(imgHeight)); + + std::string topicBase = + "/test/integration/ThermalCameraPlugin_images8BitWithBuiltinSDF/"; + std::string topic = topicBase + "image"; + WaitForMessageTestHelper helper(topic); + + std::string infoTopic = topicBase + "camera_info"; + WaitForMessageTestHelper infoHelper(infoTopic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; + + // subscribe to the thermal camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnImage8Bit); + + // subscribe to the thermal camera info topic + node.Subscribe(infoTopic, &OnCameraInfo); + + // wait for a few thermal camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + int midWidth = static_cast(thermalSensor->ImageWidth() * 0.5); + int midHeight = static_cast(thermalSensor->ImageHeight() * 0.5); + int mid = midHeight * thermalSensor->ImageWidth() + midWidth -1; + int left = midHeight * thermalSensor->ImageWidth(); + int right = (midHeight+1) * thermalSensor->ImageWidth() - 1; + + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); + int counter = 0; + int infoCounter = 0; + ignition::msgs::CameraInfo infoMsg; + for (int sleep = 0; + sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) + { + g_mutex.lock(); + counter = g_thermalCounter; + g_mutex.unlock(); + + g_infoMutex.lock(); + infoCounter = g_infoCounter; + infoMsg = g_infoMsg; + g_infoMutex.unlock(); + std::this_thread::sleep_for(waitTime); + } + g_mutex.lock(); + g_infoMutex.lock(); + g_thermalCounter = 0; + g_infoCounter = 0; + EXPECT_GT(counter, 0); + EXPECT_EQ(counter, infoCounter); + counter = 0; + infoCounter = 0; + + // verify temperature + // Box should be in the middle of image and return box temp + // Left and right side of the image frame should be ambient temp + EXPECT_NEAR(ambientTemp, g_thermalBuffer8Bit[left] * linearResolution, + ambientTempRange); + EXPECT_NEAR(ambientTemp, g_thermalBuffer8Bit[right] * linearResolution, + ambientTempRange); + EXPECT_FLOAT_EQ(g_thermalBuffer8Bit[right], g_thermalBuffer8Bit[left]); + + EXPECT_NEAR(boxTemp, g_thermalBuffer8Bit[mid] * linearResolution, + linearResolution); + + g_infoMutex.unlock(); + g_mutex.unlock(); + + // Check camera info + EXPECT_TRUE(infoMsg.has_header()); + ASSERT_EQ(1, infoMsg.header().data().size()); + EXPECT_EQ("frame_id", infoMsg.header().data(0).key()); + ASSERT_EQ(1, infoMsg.header().data(0).value().size()); + EXPECT_EQ("camera1", infoMsg.header().data(0).value(0)); + EXPECT_TRUE(infoMsg.has_distortion()); + EXPECT_EQ(ignition::msgs::CameraInfo::Distortion::PLUMB_BOB, + infoMsg.distortion().model()); + EXPECT_EQ(5, infoMsg.distortion().k().size()); + EXPECT_TRUE(infoMsg.has_intrinsics()); + EXPECT_EQ(9, infoMsg.intrinsics().k().size()); + EXPECT_TRUE(infoMsg.has_projection()); + EXPECT_EQ(12, infoMsg.projection().p().size()); + EXPECT_EQ(9, infoMsg.rectification_matrix().size()); + + // Check that for a box really close it returns box temperature + root->RemoveChild(box); + ignition::math::Vector3d boxPositionNear( + unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); + box->SetLocalPosition(boxPositionNear); + root->AddChild(box); + + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + for (int sleep = 0; + sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) + { + g_mutex.lock(); + counter = g_thermalCounter; + g_mutex.unlock(); + + g_infoMutex.lock(); + infoCounter = g_infoCounter; + g_infoMutex.unlock(); + std::this_thread::sleep_for(waitTime); + } + + g_mutex.lock(); + g_infoMutex.lock(); + g_thermalCounter = 0; + g_infoCounter = 0; + EXPECT_GT(counter, 0); + EXPECT_EQ(counter, infoCounter); + counter = 0; + infoCounter = 0; + + EXPECT_NEAR(boxTemp, g_thermalBuffer8Bit[mid] * linearResolution, + linearResolution); + g_infoMutex.unlock(); + g_mutex.unlock(); + + // Check that for a box really far it returns ambient temperature + root->RemoveChild(box); + ignition::math::Vector3d boxPositionFar( + unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); + box->SetLocalPosition(boxPositionFar); + root->AddChild(box); + + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + for (int sleep = 0; + sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) + { + g_mutex.lock(); + counter = g_thermalCounter; + g_mutex.unlock(); + + g_infoMutex.lock(); + infoCounter = g_infoCounter; + g_infoMutex.unlock(); + std::this_thread::sleep_for(waitTime); + } + g_mutex.lock(); + g_infoMutex.lock(); + g_thermalCounter = 0; + g_infoCounter = 0; + EXPECT_GT(counter, 0); + EXPECT_EQ(counter, infoCounter); + counter = 0; + infoCounter = 0; + + EXPECT_NEAR(ambientTemp, g_thermalBuffer8Bit[mid] * linearResolution, + ambientTempRange); + g_infoMutex.unlock(); + g_mutex.unlock(); + + delete [] g_thermalBuffer8Bit; + g_thermalBuffer8Bit = nullptr; + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(ThermalCameraSensorTest, Images8BitWithBuiltinSDF) +{ + Images8BitWithBuiltinSDF(GetParam()); +} + INSTANTIATE_TEST_CASE_P(ThermalCameraSensor, ThermalCameraSensorTest, RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); diff --git a/test/integration/thermal_camera_sensor_8bit_builtin.sdf b/test/integration/thermal_camera_sensor_8bit_builtin.sdf new file mode 100644 index 00000000..ff196785 --- /dev/null +++ b/test/integration/thermal_camera_sensor_8bit_builtin.sdf @@ -0,0 +1,23 @@ + + + + + + 10 + /test/integration/ThermalCameraPlugin_images8BitWithBuiltinSDF/image + + 1.05 + + 10 + 10 + L8 + + + 0.1 + 10.0 + + + + + + diff --git a/test/integration/thermal_camera_sensor_builtin.sdf b/test/integration/thermal_camera_sensor_builtin.sdf index fb2510e8..61a6d75c 100644 --- a/test/integration/thermal_camera_sensor_builtin.sdf +++ b/test/integration/thermal_camera_sensor_builtin.sdf @@ -10,7 +10,7 @@ 256 256 - L_16 + L16 0.1