fix: preserve underwater camera inputs and acquisition headers

gazebo/posim_gz_sensor_plugins/CMakeLists.txt

@@ -107,6 +107,9 @@ if(BUILD_TESTING)
   ament_add_gtest(test_underwater_camera test/test_underwater_camera.cc)
   target_include_directories(test_underwater_camera PRIVATE include)
   target_link_libraries(test_underwater_camera UnderwaterCamera)
+  ament_add_gtest(test_camera_adversarial test/test_camera_adversarial.cc)
+  target_include_directories(test_camera_adversarial PRIVATE include)
+  target_link_libraries(test_camera_adversarial UnderwaterCamera)
   ament_add_gtest(test_pressure_shutdown test/test_pressure_shutdown.cc)
   target_include_directories(test_pressure_shutdown PRIVATE include)
   target_link_libraries(test_pressure_shutdown sea_pressure_sensor)

gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc

@@ -24,6 +24,7 @@
 #include <gz/sim/components/RgbdCamera.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <limits>
 #include <sdf/Camera.hh>
 #include "gz/plugin/Register.hh"
 #include "gz/sim/EntityComponentManager.hh"
@@ -356,13 +357,16 @@ cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image
       cv_type = CV_32FC1;
       break;
     default:
-      throw std::runtime_error("Unsupported pixel format");
+      return {};
   }
 
   const size_t rowBytes = static_cast<size_t>(gz_image.width()) * CV_ELEM_SIZE(cv_type);
   const size_t stride = gz_image.step();
   if (
-    gz_image.width() == 0 || gz_image.height() == 0 || stride < rowBytes ||
+    gz_image.width() == 0 || gz_image.height() == 0 ||
+    gz_image.width() > static_cast<unsigned int>(std::numeric_limits<int>::max()) ||
+    gz_image.height() > static_cast<unsigned int>(std::numeric_limits<int>::max()) ||
+    stride < rowBytes || stride % CV_ELEM_SIZE1(cv_type) != 0 ||
     static_cast<uint64_t>(stride) * gz_image.height() > gz_image.data().size())
   {
     return {};
@@ -376,11 +380,15 @@ cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image
   // Optionally convert color space if needed (e.g., RGB to BGR)
   if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8)
   {
-    cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR);
+    cv::Mat converted;
+    cv::cvtColor(cv_image, converted, cv::COLOR_RGB2BGR);
+    return converted;
   }
   else if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8)
   {
-    cv::cvtColor(cv_image, cv_image, cv::COLOR_RGBA2BGRA);
+    cv::Mat converted;
+    cv::cvtColor(cv_image, converted, cv::COLOR_RGBA2BGRA);
+    return converted;
   }
 
   return cv_image;
@@ -439,7 +447,24 @@ void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
 
       // Publish simulated image
       sensor_msgs::msg::Image ros_image;
-      ros_image.header.stamp = this->ros_node_->now();
+      // Preserve the sensor acquisition time and frame for ROS synchronization.
+      ros_image.header.stamp.sec = msg.header().stamp().sec();
+      ros_image.header.stamp.nanosec = msg.header().stamp().nsec();
+      for (const auto & entry : msg.header().data())
+      {
+        if (entry.key() == "frame_id" && entry.value_size() > 0)
+        {
+          ros_image.header.frame_id = entry.value(0);
+          // Match ros_gz_bridge's ROS frame convention for scoped Gazebo names.
+          std::size_t separator = 0;
+          while ((separator = ros_image.header.frame_id.find("::", separator)) != std::string::npos)
+          {
+            ros_image.header.frame_id.replace(separator, 2, "/");
+            ++separator;
+          }
+          break;
+        }
+      }
       ros_image.height = msg.height();
       ros_image.width = msg.width();
       ros_image.encoding = "bgr8";
@@ -491,6 +516,12 @@ cv::Mat UnderwaterCamera::SimulateUnderwater(
       const cv::Vec3b & in = inrow[col];
       cv::Vec3b & out = outrow[col];
 
+      // NaN is an invalid/no-return depth, not a valid byte intensity.
+      // Treat it like a far return; signed infinities retain the clipping below.
+      if (std::isnan(r))
+      {
+        r = this->dataPtr->max_range;
+      }
       if (r < this->dataPtr->min_range)
       {
         r = this->dataPtr->min_range;

gazebo/posim_gz_sensor_plugins/test/test_camera_adversarial.cc

@@ -0,0 +1,179 @@
+// Copyright 2026 Kwon Yeseol
+//
+// 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.
+
+// Regression tests for malformed camera input and acquisition metadata.
+#include <gtest/gtest.h>
+#include <chrono>
+#include <cmath>
+#include <cstring>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/RgbdCamera.hh>
+#include <gz/sim/components/World.hh>
+#include <limits>
+#include <sdf/Camera.hh>
+#include <sdf/Sensor.hh>
+#include <thread>
+#include "posim_gz_sensor_plugins/UnderwaterCamera.hh"
+
+using posim_gz_sensor_plugins::UnderwaterCamera;
+namespace
+{
+gz::msgs::Image RGB(gz::msgs::PixelFormatType format = gz::msgs::RGB_INT8)
+{
+  gz::msgs::Image img;
+  img.set_width(2);
+  img.set_height(2);
+  img.set_step(6);
+  img.set_pixel_format_type(format);
+  const unsigned char bytes[] = {10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120};
+  img.set_data(bytes, sizeof(bytes));
+  return img;
+}
+class CameraInput : public ::testing::Test
+{
+protected:
+  UnderwaterCamera camera;
+  gz::sim::EntityComponentManager ecm;
+  gz::sim::EventManager events;
+  void SetUp() override
+  {
+    auto world = ecm.CreateEntity();
+    ecm.CreateComponent(world, gz::sim::components::World());
+    ecm.CreateComponent(world, gz::sim::components::Name("adversarial"));
+    sdf::Camera config;
+    config.SetImageWidth(2);
+    config.SetImageHeight(2);
+    config.SetNearClip(0.1);
+    config.SetFarClip(100);
+    config.SetLensIntrinsicsFx(2);
+    config.SetLensIntrinsicsFy(2);
+    config.SetLensIntrinsicsCx(1);
+    config.SetLensIntrinsicsCy(1);
+    sdf::Sensor sensor;
+    sensor.SetName("adversarial_camera");
+    sensor.SetType(sdf::SensorType::RGBD_CAMERA);
+    sensor.SetTopic("/adversarial_camera");
+    sensor.SetCameraSensor(config);
+    auto entity = ecm.CreateEntity();
+    ecm.CreateComponent(entity, gz::sim::components::RgbdCamera(sensor));
+    auto plugin = std::make_shared<sdf::Element>();
+    plugin->SetName("plugin");
+    camera.Configure(entity, plugin, ecm, events);
+  }
+  void TearDown() override
+  {
+    if (rclcpp::ok())
+    {
+      rclcpp::shutdown();
+    }
+  }
+  gz::msgs::Image Depth(float value = 3)
+  {
+    gz::msgs::Image d;
+    d.set_width(2);
+    d.set_height(2);
+    d.set_step(8);
+    d.set_pixel_format_type(gz::msgs::R_FLOAT32);
+    const float values[] = {value, value, value, value};
+    d.set_data(values, sizeof(values));
+    return d;
+  }
+};
+}  // namespace
+TEST(CameraConversion, RgbDoesNotModifySource)
+{
+  UnderwaterCamera c;
+  const auto img = RGB();
+  const auto before = img.SerializeAsString();
+  auto converted = c.ConvertGazeboToOpenCV(img);
+  EXPECT_EQ(before, img.SerializeAsString());
+  EXPECT_EQ(converted.at<cv::Vec3b>(0, 0), cv::Vec3b(30, 20, 10));
+}
+TEST(CameraConversion, RgbaDoesNotModifySource)
+{
+  UnderwaterCamera c;
+  auto img = RGB(gz::msgs::RGBA_INT8);
+  img.set_step(8);
+  img.set_data(std::string("\x10\x20\x30\xff", 4) + std::string(12, '\x40'));
+  const auto before = img.SerializeAsString();
+  const auto converted = c.ConvertGazeboToOpenCV(img);
+  EXPECT_EQ(before, img.SerializeAsString());
+  EXPECT_EQ(converted.at<cv::Vec4b>(0, 0), cv::Vec4b(0x30, 0x20, 0x10, 255));
+}
+TEST(CameraConversion, InvalidFloatRowAlignmentIsRejected)
+{
+  UnderwaterCamera c;
+  auto img = RGB(gz::msgs::R_FLOAT32);
+  img.set_step(9);
+  img.set_data(std::string(18, '\0'));
+  EXPECT_NO_THROW(EXPECT_TRUE(c.ConvertGazeboToOpenCV(img).empty()));
+}
+TEST(CameraConversion, UnknownFormatIsRejected)
+{
+  UnderwaterCamera c;
+  auto img = RGB(gz::msgs::UNKNOWN_PIXEL_FORMAT);
+  EXPECT_NO_THROW(EXPECT_TRUE(c.ConvertGazeboToOpenCV(img).empty()));
+}
+TEST(CameraConversion, PaddedRgbRowsRemainImmutable)
+{
+  UnderwaterCamera c;
+  auto img = RGB();
+  img.set_step(8);
+  img.set_data(
+    std::string("\x10\x20\x30\x40\x50\x60\xee\xff", 8) +
+    std::string("\x70\x80\x90\xa0\xb0\xc0\xdd\xee", 8));
+  const auto before = img.SerializeAsString();
+  auto mat = c.ConvertGazeboToOpenCV(img);
+  EXPECT_EQ(before, img.SerializeAsString());
+  EXPECT_EQ(mat.at<cv::Vec3b>(1, 0), cv::Vec3b(0x90, 0x80, 0x70));
+}
+TEST_F(CameraInput, MalformedTransportInputDoesNotThrow)
+{
+  camera.DepthImageCallback(Depth());
+  camera.CameraCallback(RGB());
+  EXPECT_NO_THROW(camera.CameraCallback(RGB(gz::msgs::UNKNOWN_PIXEL_FORMAT)));
+  auto bad = Depth();
+  bad.set_step(9);
+  bad.set_data(std::string(18, '\0'));
+  camera.DepthImageCallback(bad);
+  EXPECT_NO_THROW(camera.CameraCallback(RGB()));
+}
+TEST_F(CameraInput, NonfiniteDepthUsesDefinedClipping)
+{
+  unsetenv("POSIM_UNDERWATER_PLUGIN");
+  cv::Mat rgb(2, 2, CV_8UC3, cv::Scalar(90, 120, 150));
+  cv::Mat depth(2, 2, CV_32FC1, cv::Scalar(3));
+  cv::Mat out(2, 2, CV_8UC3);
+  depth.at<float>(1, 1) = std::numeric_limits<float>::quiet_NaN();
+  camera.SimulateUnderwater(rgb, depth, out);
+  for (int c = 0; c < 3; ++c)
+  {
+    EXPECT_NEAR(out.at<cv::Vec3b>(1, 1)[c], (90 + c * 30) * std::exp(-100.0 / 30.0), 1.0);
+  }
+  depth.at<float>(1, 1) = std::numeric_limits<float>::infinity();
+  EXPECT_NO_THROW(camera.SimulateUnderwater(rgb, depth, out));
+  depth.at<float>(1, 1) = -std::numeric_limits<float>::infinity();
+  camera.SimulateUnderwater(rgb, depth, out);
+  EXPECT_NEAR(out.at<cv::Vec3b>(1, 1)[0], 90 * std::exp(-0.1 / 30.0), 1.0);
+}
+TEST_F(CameraInput, PassthroughDoesNotApplyOpticsTwice)
+{
+  setenv("POSIM_UNDERWATER_PLUGIN", "1", 1);
+  cv::Mat rgb(2, 2, CV_8UC3, cv::Scalar(90, 120, 150));
+  cv::Mat depth(2, 2, CV_32FC1, cv::Scalar(30));
+  cv::Mat out(2, 2, CV_8UC3);
+  camera.SimulateUnderwater(rgb, depth, out);
+  EXPECT_EQ(cv::norm(rgb, out, cv::NORM_INF), 0);
+  unsetenv("POSIM_UNDERWATER_PLUGIN");
+}

gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc

@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+#include <chrono>
 #include <csignal>
 #include <cstring>
 #include <gz/sim/EntityComponentManager.hh>
@@ -8,6 +9,7 @@
 #include <gz/sim/components/World.hh>
 #include <sdf/Camera.hh>
 #include <sdf/Sensor.hh>
+#include <thread>
 
 #include "posim_gz_sensor_plugins/UnderwaterCamera.hh"
 
@@ -128,3 +130,87 @@ TEST(UnderwaterCamera, ConfigurePreservesSignalHandlerAndMissingDepthIsSafe)
   }
   std::signal(SIGINT, previous);
 }
+
+TEST(UnderwaterCamera, PreservesAcquisitionHeader)
+{
+  UnderwaterCamera camera;
+  gz::sim::EntityComponentManager ecm;
+  gz::sim::EventManager events;
+  auto world = ecm.CreateEntity();
+  ecm.CreateComponent(world, gz::sim::components::World());
+  ecm.CreateComponent(world, gz::sim::components::Name("camera_test"));
+  sdf::Camera config;
+  config.SetImageWidth(2);
+  config.SetImageHeight(2);
+  config.SetNearClip(0.1);
+  config.SetFarClip(100);
+  config.SetLensIntrinsicsFx(2);
+  config.SetLensIntrinsicsFy(2);
+  config.SetLensIntrinsicsCx(1);
+  config.SetLensIntrinsicsCy(1);
+  sdf::Sensor sensor;
+  sensor.SetName("test_camera");
+  sensor.SetType(sdf::SensorType::RGBD_CAMERA);
+  sensor.SetTopic("/camera_unit_test");
+  sensor.SetCameraSensor(config);
+  auto entity = ecm.CreateEntity();
+  ecm.CreateComponent(entity, gz::sim::components::RgbdCamera(sensor));
+  auto plugin = std::make_shared<sdf::Element>();
+  plugin->SetName("plugin");
+  camera.Configure(entity, plugin, ecm, events);
+
+  auto node = std::make_shared<rclcpp::Node>("camera_header_observer");
+  sensor_msgs::msg::Image::SharedPtr received;
+  auto subscription = node->create_subscription<sensor_msgs::msg::Image>(
+    "/camera_unit_test/simulated_image", rclcpp::QoS(10),
+    [&received](sensor_msgs::msg::Image::SharedPtr image) { received = image; });
+  gz::msgs::Image depth;
+  depth.set_width(2);
+  depth.set_height(2);
+  depth.set_step(8);
+  depth.set_pixel_format_type(gz::msgs::PixelFormatType::R_FLOAT32);
+  const float values[] = {3, 3, 3, 3};
+  depth.set_data(values, sizeof(values));
+  camera.DepthImageCallback(depth);
+  gz::msgs::Image rgb;
+  rgb.set_width(2);
+  rgb.set_height(2);
+  rgb.set_step(6);
+  rgb.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8);
+  rgb.set_data(std::string(12, '\x20'));
+  rgb.mutable_header()->mutable_stamp()->set_sec(42);
+  rgb.mutable_header()->mutable_stamp()->set_nsec(123456789);
+  auto frame = rgb.mutable_header()->add_data();
+  frame->set_key("frame_id");
+  frame->add_value("vehicle::camera::optical");
+  for (int i = 0; i < 150 && !received; ++i)
+  {
+    camera.CameraCallback(rgb);
+    rclcpp::spin_some(node);
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+  ASSERT_TRUE(received);
+  EXPECT_EQ(received->header.stamp.sec, 42);
+  EXPECT_EQ(received->header.stamp.nanosec, 123456789u);
+  EXPECT_EQ(received->header.frame_id, "vehicle/camera/optical");
+  EXPECT_EQ(received->encoding, "bgr8");
+  EXPECT_EQ(received->data.size(), 12u);
+  // A time of zero is valid at simulation start; do not substitute wall time.
+  received.reset();
+  rgb.clear_header();
+  for (int i = 0; i < 150; ++i)
+  {
+    camera.CameraCallback(rgb);
+    rclcpp::spin_some(node);
+    if (received && received->header.stamp.sec == 0)
+    {
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+  ASSERT_TRUE(received);
+  EXPECT_EQ(received->header.stamp.sec, 0);
+  EXPECT_EQ(received->header.stamp.nanosec, 0u);
+  EXPECT_TRUE(received->header.frame_id.empty());
+  rclcpp::shutdown();
+}