diff --git a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt b/gazebo/posim_gz_sensor_plugins/CMakeLists.txt index 1f83f32e..9c87f497 100644 --- a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt +++ b/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() diff --git a/gazebo/posim_gz_sensor_plugins/include/posim_gz_sensor_plugins/UnderwaterCamera.hh b/gazebo/posim_gz_sensor_plugins/include/posim_gz_sensor_plugins/UnderwaterCamera.hh index 553a53ed..36c137f2 100644 --- a/gazebo/posim_gz_sensor_plugins/include/posim_gz_sensor_plugins/UnderwaterCamera.hh +++ b/gazebo/posim_gz_sensor_plugins/include/posim_gz_sensor_plugins/UnderwaterCamera.hh @@ -62,6 +62,11 @@ private: struct PrivateData; std::unique_ptr 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; }; } // namespace posim_gz_sensor_plugins diff --git a/gazebo/posim_gz_sensor_plugins/package.xml b/gazebo/posim_gz_sensor_plugins/package.xml index fe29ef56..38a17b13 100644 --- a/gazebo/posim_gz_sensor_plugins/package.xml +++ b/gazebo/posim_gz_sensor_plugins/package.xml @@ -12,6 +12,7 @@ geometry_msgs ament_cmake ament_lint_auto + ament_cmake_gtest posim_interfaces protobuf protobuf diff --git a/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc index 2b09bb80..86e28889 100644 --- a/gazebo/posim_gz_sensor_plugins/src/UnderwaterCamera.cc +++ b/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()) {} +struct UnderwaterCamera::CallbackState +{ + std::mutex mutex; + UnderwaterCamera * owner = nullptr; +}; + +UnderwaterCamera::UnderwaterCamera() +: dataPtr(std::make_unique()), callbackState(std::make_shared()) +{ + 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 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("backgroundB"); } - // Gazebo camera subscriber + // Construct the publisher before making callbacks visible to transport. + this->dataPtr->image_pub = this->ros_node_->create_publisher( + this->dataPtr->simulated_image_topic, 1); + + // Capture the lifetime gate, not an unguarded `this` pointer. + const auto state = this->callbackState; std::function camera_callback = - std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1); + [state](const gz::msgs::Image & image) + { + std::lock_guard 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 depth_callback = - std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1); + [state](const gz::msgs::Image & image) + { + std::lock_guard 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( - 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(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(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(reinterpret_cast(gz_image.data().data()))); + const_cast(reinterpret_cast(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 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) { diff --git a/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc b/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc new file mode 100644 index 00000000..eaaaa532 --- /dev/null +++ b/gazebo/posim_gz_sensor_plugins/test/test_underwater_camera.cc @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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(1, 0), 3); + EXPECT_FLOAT_EQ(mat.at(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(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(); + 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(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); +}