diff --git a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt b/gazebo/posim_gz_sensor_plugins/CMakeLists.txt index e549f4f9..4eecfc46 100644 --- a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt +++ b/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) diff --git a/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc index 864c2096..2a137c2d 100644 --- a/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc +++ b/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #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(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(std::numeric_limits::max()) || + gz_image.height() > static_cast(std::numeric_limits::max()) || + stride < rowBytes || stride % CV_ELEM_SIZE1(cv_type) != 0 || static_cast(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; diff --git a/gazebo/posim_gz_sensor_plugins/test/test_camera_adversarial.cc b/gazebo/posim_gz_sensor_plugins/test/test_camera_adversarial.cc new file mode 100644 index 00000000..4477f78c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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(); + 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(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(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(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(1, 1) = std::numeric_limits::quiet_NaN(); + camera.SimulateUnderwater(rgb, depth, out); + for (int c = 0; c < 3; ++c) + { + EXPECT_NEAR(out.at(1, 1)[c], (90 + c * 30) * std::exp(-100.0 / 30.0), 1.0); + } + depth.at(1, 1) = std::numeric_limits::infinity(); + EXPECT_NO_THROW(camera.SimulateUnderwater(rgb, depth, out)); + depth.at(1, 1) = -std::numeric_limits::infinity(); + camera.SimulateUnderwater(rgb, depth, out); + EXPECT_NEAR(out.at(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"); +} diff --git a/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc b/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc index eaaaa532..b9e3080d 100644 --- a/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc +++ b/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -8,6 +9,7 @@ #include #include #include +#include #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(); + plugin->SetName("plugin"); + camera.Configure(entity, plugin, ecm, events); + + auto node = std::make_shared("camera_header_observer"); + sensor_msgs::msg::Image::SharedPtr received; + auto subscription = node->create_subscription( + "/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(); +}