Add support for 16 bit image format (#276)

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

Co-authored-by: Michael Carroll <michael@openrobotics.org>

src/CameraSensor.cc

@@ -220,6 +220,9 @@ bool CameraSensor::CreateCamera()
     case sdf::PixelFormatType::L_INT8:
       this->dataPtr->camera->SetImageFormat(ignition::rendering::PF_L8);
       break;
+    case sdf::PixelFormatType::L_INT16:
+      this->dataPtr->camera->SetImageFormat(ignition::rendering::PF_L16);
+      break;
     default:
       ignerr << "Unsupported pixel format ["
         << static_cast<int>(pixelFormat) << "]\n";
@@ -447,6 +450,10 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
       format = ignition::common::Image::L_INT8;
       msgsPixelFormat = msgs::PixelFormatType::L_INT8;
       break;
+    case ignition::rendering::PF_L16:
+      format = ignition::common::Image::L_INT16;
+      msgsPixelFormat = msgs::PixelFormatType::L_INT16;
+      break;
     default:
       ignerr << "Unsupported pixel format ["
         << this->dataPtr->camera->ImageFormat() << "]\n";

test/integration/camera.cc

@@ -51,7 +51,7 @@
 std::mutex g_mutex;
 unsigned int g_imgCounter = 0;
 
-void OnGrayscaleImage(const ignition::msgs::Image &_msg)
+void OnGrayscale8bitImage(const ignition::msgs::Image &_msg)
 {
   std::lock_guard<std::mutex> lock(g_mutex);
   EXPECT_EQ(ignition::msgs::PixelFormatType::L_INT8, _msg.pixel_format_type());
@@ -60,6 +60,15 @@ void OnGrayscaleImage(const ignition::msgs::Image &_msg)
   g_imgCounter++;
 }
 
+void OnGrayscale16bitImage(const ignition::msgs::Image &_msg)
+{
+  std::lock_guard<std::mutex> lock(g_mutex);
+  EXPECT_EQ(ignition::msgs::PixelFormatType::L_INT16, _msg.pixel_format_type());
+  EXPECT_EQ(256u, _msg.width());
+  EXPECT_EQ(256u, _msg.height());
+  g_imgCounter++;
+}
+
 class CameraSensorTest: public testing::Test,
   public testing::WithParamInterface<const char *>
 {
@@ -74,6 +83,9 @@ class CameraSensorTest: public testing::Test,
 
   // Create a 8 bit grayscale camera sensor and verify image format
   public: void ImageFormatLInt8(const std::string &_renderEngine);
+
+  // Create a 16 bit grayscale camera sensor and verify image format
+  public: void ImageFormatLInt16(const std::string &_renderEngine);
 };
 
 void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine)
@@ -217,9 +229,11 @@ void CameraSensorTest::ImageFormatLInt8(const std::string &_renderEngine)
 
   EXPECT_TRUE(helper.WaitForMessage()) << helper;
 
+  g_imgCounter = 0u;
+
   // subscribe to the camera topic
   ignition::transport::Node node;
-  node.Subscribe(topic, &OnGrayscaleImage);
+  node.Subscribe(topic, &OnGrayscale8bitImage);
 
   // wait for a few camera frames
   mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true);
@@ -258,6 +272,98 @@ TEST_P(CameraSensorTest, LInt8ImagesWithBuiltinSDF)
   ImageFormatLInt8(GetParam());
 }
 
+//////////////////////////////////////////////////
+void CameraSensorTest::ImageFormatLInt16(const std::string &_renderEngine)
+{
+  // get the darn test data
+  std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test",
+      "sdf", "camera_sensor_l16_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");
+
+  // Setup 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");
+
+  // do the test
+  ignition::sensors::Manager mgr;
+
+  ignition::sensors::CameraSensor *sensor =
+      mgr.CreateSensor<ignition::sensors::CameraSensor>(sensorPtr);
+  ASSERT_NE(sensor, nullptr);
+  sensor->SetScene(scene);
+
+  ASSERT_NE(sensor->RenderingCamera(), nullptr);
+  EXPECT_NE(sensor->Id(), sensor->RenderingCamera()->Id());
+  EXPECT_EQ(256u, sensor->ImageWidth());
+  EXPECT_EQ(256u, sensor->ImageHeight());
+
+  std::string topic = "/images_l16";
+  WaitForMessageTestHelper<ignition::msgs::Image> helper(topic);
+
+  // Update once to create image
+  mgr.RunOnce(std::chrono::steady_clock::duration::zero());
+
+  EXPECT_TRUE(helper.WaitForMessage()) << helper;
+
+  g_imgCounter = 0u;
+
+  // subscribe to the camera topic
+  ignition::transport::Node node;
+  node.Subscribe(topic, &OnGrayscale16bitImage);
+
+  // wait for a few camera frames
+  mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true);
+
+  // run to get image and check image format in callback
+  bool done = false;
+  int sleep = 0;
+  int maxSleep = 10;
+  while (!done && sleep++ < maxSleep)
+  {
+    std::lock_guard<std::mutex> lock(g_mutex);
+    done = (g_imgCounter > 0);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  // test removing sensor
+  // first make sure the sensor objects do exist
+  auto sensorId = sensor->Id();
+  auto cameraId = sensor->RenderingCamera()->Id();
+  EXPECT_EQ(sensor, mgr.Sensor(sensorId));
+  EXPECT_EQ(sensor->RenderingCamera(), scene->SensorById(cameraId));
+  // remove and check sensor objects no longer exist in both sensors and
+  // rendering
+  EXPECT_TRUE(mgr.Remove(sensorId));
+  EXPECT_EQ(nullptr, mgr.Sensor(sensorId));
+  EXPECT_EQ(nullptr, scene->SensorById(cameraId));
+
+  // Clean up
+  engine->DestroyScene(scene);
+  ignition::rendering::unloadEngine(engine->Name());
+}
+
+//////////////////////////////////////////////////
+TEST_P(CameraSensorTest, LInt16ImagesWithBuiltinSDF)
+{
+  ImageFormatLInt16(GetParam());
+}
+
 INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest,
     RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam());
 

test/sdf/camera_sensor_l16_builtin.sdf

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="m1">
+    <link name="link1">
+      <sensor name="camera1" type="camera">
+        <update_rate>10</update_rate>
+        <ignition_frame_id>base_camera</ignition_frame_id>
+        <topic>/images_l16</topic>
+        <camera>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>256</width>
+            <height>256</height>
+            <format>L_INT16</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>10.0</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+    </link>
+  </model>
+</sdf>