fix: handle invalid camera frames and preserve Gazebo signal handling

gazebo/posim_gz_sensor_plugins/CMakeLists.txt

@@ -103,6 +103,10 @@ ament_environment_hooks(
 
 # Testing setup
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  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)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()

gazebo/posim_gz_sensor_plugins/include/posim_gz_sensor_plugins/UnderwaterCamera.hh

@@ -62,6 +62,11 @@ private:
 
   struct PrivateData;
   std::unique_ptr<PrivateData> dataPtr;
+
+  // Queued transport callbacks may outlive Unsubscribe(). They must not retain
+  // an unguarded pointer to the plugin after its destructor starts.
+  struct CallbackState;
+  std::shared_ptr<CallbackState> callbackState;
 };
 }  // namespace posim_gz_sensor_plugins
 

gazebo/posim_gz_sensor_plugins/package.xml

@@ -12,6 +12,7 @@
   <depend>geometry_msgs</depend>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <build_depend>posim_interfaces</build_depend>
   <build_depend>protobuf</build_depend>
   <exec_depend>protobuf</exec_depend>

gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc

@@ -73,7 +73,7 @@ struct UnderwaterCamera::PrivateData
   gz::msgs::Image lastImage;
 
   /// \brief Depth to range lookup table (LUT)
-  float * depth2rangeLUT;
+  float * depth2rangeLUT = nullptr;
 
   /// \brief Attenuation constants per channel (RGB)
   float attenuation[3];
@@ -87,10 +87,34 @@ struct UnderwaterCamera::PrivateData
   float max_range;
 };
 
-UnderwaterCamera::UnderwaterCamera() : dataPtr(std::make_unique<PrivateData>()) {}
+struct UnderwaterCamera::CallbackState
+{
+  std::mutex mutex;
+  UnderwaterCamera * owner = nullptr;
+};
+
+UnderwaterCamera::UnderwaterCamera()
+: dataPtr(std::make_unique<PrivateData>()), callbackState(std::make_shared<CallbackState>())
+{
+  this->callbackState->owner = this;
+}
 
 UnderwaterCamera::~UnderwaterCamera()
 {
+  // Drain an active callback before freeing the LUT, and make already-queued
+  // callbacks harmless. Do not hold this lock while calling transport APIs.
+  {
+    std::lock_guard<std::mutex> lock(this->callbackState->mutex);
+    this->callbackState->owner = nullptr;
+  }
+  if (!this->dataPtr->image_topic.empty())
+  {
+    this->dataPtr->gz_node.Unsubscribe(this->dataPtr->image_topic);
+  }
+  if (!this->dataPtr->depth_image_topic.empty())
+  {
+    this->dataPtr->gz_node.Unsubscribe(this->dataPtr->depth_image_topic);
+  }
   if (this->dataPtr->depth2rangeLUT)
   {
     delete[] this->dataPtr->depth2rangeLUT;
@@ -128,7 +152,9 @@ void UnderwaterCamera::Configure(
 
   if (!rclcpp::ok())
   {
-    rclcpp::init(0, nullptr);
+    // Gazebo owns process signals. A plugin must not replace its handlers or
+    // invalidate ROS while Gazebo is still running update/transport callbacks.
+    rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None);
   }
 
   std::string rosNodeName = sensorSdf.Name() + "_node";
@@ -276,21 +302,36 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[0] = (unsigned char)_sdf->Get<int>("backgroundB");
   }
 
-  // Gazebo camera subscriber
+  // Construct the publisher before making callbacks visible to transport.
+  this->dataPtr->image_pub = this->ros_node_->create_publisher<sensor_msgs::msg::Image>(
+    this->dataPtr->simulated_image_topic, 1);
+
+  // Capture the lifetime gate, not an unguarded `this` pointer.
+  const auto state = this->callbackState;
   std::function<void(const gz::msgs::Image &)> camera_callback =
-    std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
+    [state](const gz::msgs::Image & image)
+  {
+    std::lock_guard<std::mutex> lock(state->mutex);
+    if (state->owner)
+    {
+      state->owner->CameraCallback(image);
+    }
+  };
 
   this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback);
 
   // Gazebo depth image subscriber
   std::function<void(const gz::msgs::Image &)> depth_callback =
-    std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1);
+    [state](const gz::msgs::Image & image)
+  {
+    std::lock_guard<std::mutex> lock(state->mutex);
+    if (state->owner)
+    {
+      state->owner->DepthImageCallback(image);
+    }
+  };
 
   this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback);
-
-  // ROS2 publisher
-  this->dataPtr->image_pub = this->ros_node_->create_publisher<sensor_msgs::msg::Image>(
-    this->dataPtr->simulated_image_topic, 1);
 }
 
 cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image)
@@ -317,10 +358,19 @@ cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image
       throw std::runtime_error("Unsupported pixel format");
   }
 
-  // Create OpenCV Mat header that uses the same memory as the Gazebo image data
+  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 ||
+    static_cast<uint64_t>(stride) * gz_image.height() > gz_image.data().size())
+  {
+    return {};
+  }
+
+  // Respect Gazebo's row stride, including padded depth images.
   cv::Mat cv_image(
     gz_image.height(), gz_image.width(), cv_type,
-    const_cast<void *>(reinterpret_cast<const void *>(gz_image.data().data())));
+    const_cast<void *>(reinterpret_cast<const void *>(gz_image.data().data())), stride);
 
   // Optionally convert color space if needed (e.g., RGB to BGR)
   if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8)
@@ -339,6 +389,10 @@ void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
 
+  if (!this->ros_node_ || !rclcpp::ok(this->ros_node_->get_node_base_interface()->get_context()))
+  {
+    return;
+  }
   if (!this->dataPtr->depth2rangeLUT)
   {
     gzerr << "Depth2range LUT not initialized" << std::endl;
@@ -355,14 +409,29 @@ void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
     }
     else
     {
+      // RGB and depth arrive independently. Do not index a missing, truncated,
+      // or differently-sized depth frame (including during startup/teardown).
+      const auto & depth = this->dataPtr->lastDepth;
+      if (
+        depth.pixel_format_type() != gz::msgs::PixelFormatType::R_FLOAT32 ||
+        depth.width() != this->dataPtr->width || depth.height() != this->dataPtr->height ||
+        msg.width() != this->dataPtr->width || msg.height() != this->dataPtr->height)
+      {
+        return;
+      }
       // Convert Gazebo image to OpenCV image
       cv::Mat image = this->ConvertGazeboToOpenCV(msg);
 
       // Convert depth image to OpenCV image using the ConvertGazeboToOpenCV function
       cv::Mat depth_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastDepth);
 
-      // Create output image
-      cv::Mat output_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastImage);
+      if (image.empty() || image.type() != CV_8UC3 || depth_image.empty())
+      {
+        return;
+      }
+
+      // Own the output buffer instead of writing through a protobuf const view.
+      cv::Mat output_image(image.rows, image.cols, CV_8UC3);
 
       // Simulate underwater
       cv::Mat simulated_image = this->SimulateUnderwater(image, depth_image, output_image);
@@ -440,9 +509,31 @@ cv::Mat UnderwaterCamera::SimulateUnderwater(
 void UnderwaterCamera::PostUpdate(
   const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
 {
+  if (!this->ros_node_)
+  {
+    return;
+  }
+  const auto context = this->ros_node_->get_node_base_interface()->get_context();
+  if (!rclcpp::ok(context))
+  {
+    return;
+  }
   if (!_info.paused)
   {
-    rclcpp::spin_some(this->ros_node_);
+    try
+    {
+      rclcpp::spin_some(this->ros_node_);
+    }
+    catch (const rclcpp::exceptions::RCLError &)
+    {
+      // Another embedded ROS component may shut down the shared context.
+      // Do not mask an error while the context is still valid.
+      if (rclcpp::ok(context))
+      {
+        throw;
+      }
+      return;
+    }
 
     if (_info.iterations % 1000 == 0)
     {

gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc

@@ -0,0 +1,130 @@
+#include <gtest/gtest.h>
+#include <csignal>
+#include <cstring>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/RgbdCamera.hh>
+#include <gz/sim/components/World.hh>
+#include <sdf/Camera.hh>
+#include <sdf/Sensor.hh>
+
+#include "posim_gz_sensor_plugins/UnderwaterCamera.hh"
+
+using posim_gz_sensor_plugins::UnderwaterCamera;
+
+TEST(UnderwaterCamera, UnconfiguredUpdateAndDestruction)
+{
+  UnderwaterCamera camera;
+  gz::sim::EntityComponentManager ecm;
+  gz::sim::UpdateInfo info;
+  camera.PostUpdate(info, ecm);
+}
+
+TEST(UnderwaterCamera, PaddedDepthRows)
+{
+  UnderwaterCamera camera;
+  gz::msgs::Image image;
+  image.set_width(2);
+  image.set_height(2);
+  image.set_step(12);
+  image.set_pixel_format_type(gz::msgs::PixelFormatType::R_FLOAT32);
+  const float data[] = {1, 2, 99, 3, 4, 99};
+  image.set_data(data, sizeof(data));
+  const auto mat = camera.ConvertGazeboToOpenCV(image);
+  ASSERT_FALSE(mat.empty());
+  EXPECT_FLOAT_EQ(mat.at<float>(1, 0), 3);
+  EXPECT_FLOAT_EQ(mat.at<float>(1, 1), 4);
+}
+
+TEST(UnderwaterCamera, TruncatedOrMissingImageIsRejected)
+{
+  UnderwaterCamera camera;
+  gz::msgs::Image image;
+  image.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8);
+  EXPECT_TRUE(camera.ConvertGazeboToOpenCV(image).empty());
+  image.set_width(2);
+  image.set_height(2);
+  image.set_step(6);
+  image.set_data(std::string(11, '\0'));
+  EXPECT_TRUE(camera.ConvertGazeboToOpenCV(image).empty());
+  image.set_step(5);
+  image.set_data(std::string(12, '\0'));
+  EXPECT_TRUE(camera.ConvertGazeboToOpenCV(image).empty());
+}
+
+TEST(UnderwaterCamera, RgbToBgrIsUnchanged)
+{
+  UnderwaterCamera camera;
+  gz::msgs::Image image;
+  image.set_width(1);
+  image.set_height(1);
+  image.set_step(3);
+  image.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8);
+  const unsigned char data[] = {10, 20, 30};
+  image.set_data(data, sizeof(data));
+  const auto mat = camera.ConvertGazeboToOpenCV(image);
+  ASSERT_FALSE(mat.empty());
+  EXPECT_EQ(mat.at<cv::Vec3b>(0, 0), cv::Vec3b(30, 20, 10));
+}
+
+TEST(UnderwaterCamera, ConfigurePreservesSignalHandlerAndMissingDepthIsSafe)
+{
+  // A Gazebo plugin must not take ownership of the enclosing process's SIGINT.
+  auto handler = +[](int) {};
+  auto previous = std::signal(SIGINT, handler);
+  {
+    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);
+    EXPECT_EQ(std::signal(SIGINT, handler), handler);
+
+    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'));
+    camera.CameraCallback(rgb);
+    camera.CameraCallback(rgb);  // No depth has arrived; must not index it.
+
+    cv::Mat color(2, 2, CV_8UC3, cv::Scalar(90, 120, 150));
+    cv::Mat depth(2, 2, CV_32FC1, cv::Scalar(3));
+    cv::Mat output(2, 2, CV_8UC3);
+    camera.SimulateUnderwater(color, depth, output);
+    // At the principal point, range equals depth. Default attenuation is 1/30.
+    for (int channel = 0; channel < 3; ++channel)
+    {
+      const auto expected = (90 + 30 * channel) * std::exp(-3.0 / 30.0);
+      EXPECT_NEAR(output.at<cv::Vec3b>(1, 1)[channel], expected, 1.0);
+    }
+
+    // Explicit shutdown by another component must also be safe for PostUpdate.
+    rclcpp::shutdown();
+    gz::sim::UpdateInfo info;
+    camera.PostUpdate(info, ecm);
+  }
+  std::signal(SIGINT, previous);
+}