fix: guard ROS plugin updates during context shutdown

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_pressure_shutdown test/test_pressure_shutdown.cc)
+  target_include_directories(test_pressure_shutdown PRIVATE include)
+  target_link_libraries(test_pressure_shutdown sea_pressure_sensor)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()

gazebo/posim_gz_sensor_plugins/src/UsblTransceiver.cc

@@ -110,7 +110,8 @@ void UsblTransceiver::Configure(
 
   if (!rclcpp::ok())
   {
-    rclcpp::init(0, nullptr);
+    // Gazebo owns process signals; do not invalidate ROS during its final updates.
+    rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None);
   }
 
   this->ros_node_ = std::make_shared<rclcpp::Node>("usbl_transceiver_node");
@@ -601,32 +602,56 @@ void UsblTransceiver::calculateRelativePose(
 void UsblTransceiver::PostUpdate(
   const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
 {
-  // ROS callbacks are wall-time events and must remain responsive while paused.
-  rclcpp::spin_some(this->ros_node_);
-  if (!_info.paused)
+  if (!this->ros_node_)
   {
-    if (!this->dataPtr->setGlobalMode)
+    return;
+  }
+  const auto context = this->ros_node_->get_node_base_interface()->get_context();
+  if (!rclcpp::ok(context))
+  {
+    return;
+  }
+  try
+  {
+    // ROS callbacks are wall-time events and must remain responsive while paused.
+    rclcpp::spin_some(this->ros_node_);
+    if (!rclcpp::ok(context))
     {
-      std_msgs::msg::String mode;
-      mode.data = this->dataPtr->m_interrogationMode;
-
-      this->dataPtr->m_interrogationModePub->publish(mode);
-      gzmsg << "Published mode" << std::endl;
-      this->dataPtr->setGlobalMode = true;
+      return;
     }
-
-    if (this->dataPtr->m_enablePingerScheduler)
+    if (!_info.paused)
     {
-      // Calculate the time interval between pings based on frequency
-      double pingInterval = 1.0 / this->dataPtr->m_pingFrequency;
+      if (!this->dataPtr->setGlobalMode)
+      {
+        std_msgs::msg::String mode;
+        mode.data = this->dataPtr->m_interrogationMode;
+
+        this->dataPtr->m_interrogationModePub->publish(mode);
+        gzmsg << "Published mode" << std::endl;
+        this->dataPtr->setGlobalMode = true;
+      }
 
-      // Check if it's time to send a ping
-      if (_info.iterations % static_cast<int>(pingInterval * 1000) == 0)
+      if (this->dataPtr->m_enablePingerScheduler)
       {
-        sendPing();
+        // Calculate the time interval between pings based on frequency
+        double pingInterval = 1.0 / this->dataPtr->m_pingFrequency;
+
+        // Check if it's time to send a ping
+        if (_info.iterations % static_cast<int>(pingInterval * 1000) == 0)
+        {
+          sendPing();
+        }
       }
     }
   }
+  catch (const rclcpp::exceptions::RCLError &)
+  {
+    // Suppress only shutdown races; preserve errors while the context is valid.
+    if (rclcpp::ok(context))
+    {
+      throw;
+    }
+  }
 }
 
 }  // namespace posim_gz_sensor_plugins

gazebo/posim_gz_sensor_plugins/src/UsblTransponder.cc

@@ -92,7 +92,8 @@ void UsblTransponder::Configure(
 
   if (!rclcpp::ok())
   {
-    rclcpp::init(0, nullptr);
+    // Gazebo owns process signals; do not invalidate ROS during its final updates.
+    rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None);
   }
 
   this->dataPtr->ecm = &_ecm;
@@ -393,8 +394,32 @@ void UsblTransponder::commandRosCallback(const posim_interfaces::msg::UsblComman
 void UsblTransponder::PostUpdate(
   const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
 {
-  // ROS callbacks are wall-time events and must remain responsive while paused.
-  rclcpp::spin_some(this->ros_node_);
+  if (!this->ros_node_)
+  {
+    return;
+  }
+  const auto context = this->ros_node_->get_node_base_interface()->get_context();
+  if (!rclcpp::ok(context))
+  {
+    return;
+  }
+  try
+  {
+    // ROS callbacks are wall-time events and must remain responsive while paused.
+    rclcpp::spin_some(this->ros_node_);
+    if (!rclcpp::ok(context))
+    {
+      return;
+    }
+  }
+  catch (const rclcpp::exceptions::RCLError &)
+  {
+    // Suppress only shutdown races; preserve errors while the context is valid.
+    if (rclcpp::ok(context))
+    {
+      throw;
+    }
+  }
 }
 
 }  // namespace posim_gz_sensor_plugins

gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc

@@ -67,7 +67,7 @@ void SubseaPressureSensorPlugin::Configure(
 {
   if (!rclcpp::ok())
   {
-    rclcpp::init(0, nullptr);
+    rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None);
   }
 
   gzdbg << "posim_gz_sensor_plugins::SubseaPressureSensorPlugin::Configure on entity: " << _entity
@@ -217,64 +217,86 @@ void SubseaPressureSensorPlugin::PreUpdate(
 void SubseaPressureSensorPlugin::PostUpdate(
   const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
 {
-  if (this->dataPtr->hasMeasurement && this->dataPtr->updateRate > 0.0)
+  if (!this->rosNode || !this->dataPtr->ros_pressure_sensor_pub)
   {
-    const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
-      std::chrono::duration<double>(1.0 / this->dataPtr->updateRate));
-    if (
-      _info.simTime >= this->dataPtr->lastMeasurementTime &&
-      _info.simTime - this->dataPtr->lastMeasurementTime < period)
-    {
-      return;
-    }
+    return;
   }
-
-  this->dataPtr->lastMeasurementTime = _info.simTime;
-  this->dataPtr->hasMeasurement = true;
-
-  // The pressure model uses kPa internally, while both Gazebo and ROS
-  // FluidPressure messages require Pa (and Pa^2 for variance).
-  constexpr double kPaToPa = 1000.0;
-  const double pressurePa = this->dataPtr->pressure * kPaToPa;
-  const double variancePa2 =
-    this->dataPtr->noiseSigma * this->dataPtr->noiseSigma * kPaToPa * kPaToPa;
-
-  // Publishing Sea_Pressure and depth estimate on gazebo topic
-  gz::msgs::FluidPressure gzPressureMsg;
-  gzPressureMsg.set_pressure(pressurePa);
-  gzPressureMsg.set_variance(variancePa2);
-
-  // Publishing the pressure message
-  this->dataPtr->gz_pressure_sensor_pub.Publish(gzPressureMsg);
-
-  // Publishing Sea_Pressure on Ros Topic
-  sensor_msgs::msg::FluidPressure rosPressureMsg;
-  rosPressureMsg.header.stamp.sec =
-    std::chrono::duration_cast<std::chrono::seconds>(_info.simTime).count();  // Time in seconds
-  rosPressureMsg.header.stamp.nanosec =
-    std::chrono::duration_cast<std::chrono::nanoseconds>(_info.simTime).count() %
-    1000000000;  // Time in nanoseconds
-  rosPressureMsg.fluid_pressure = pressurePa;
-  rosPressureMsg.variance = variancePa2;
-  this->dataPtr->ros_pressure_sensor_pub->publish(rosPressureMsg);
-
-  // publishing depth message
-  if (this->dataPtr->estimateDepth)
+  const auto context = this->rosNode->get_node_base_interface()->get_context();
+  if (!rclcpp::ok(context))
   {
-    geometry_msgs::msg::PointStamped rosDepthMsg;
-    rosDepthMsg.point.z = this->dataPtr->inferredDepth;
-    rosDepthMsg.header.stamp.sec =
-      std::chrono::duration_cast<std::chrono::seconds>(this->dataPtr->lastMeasurementTime).count();
-    this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg);
+    return;
   }
-
-  if (!_info.paused)
+  try
   {
-    rclcpp::spin_some(this->rosNode);
+    if (this->dataPtr->hasMeasurement && this->dataPtr->updateRate > 0.0)
+    {
+      const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+        std::chrono::duration<double>(1.0 / this->dataPtr->updateRate));
+      if (
+        _info.simTime >= this->dataPtr->lastMeasurementTime &&
+        _info.simTime - this->dataPtr->lastMeasurementTime < period)
+      {
+        return;
+      }
+    }
 
-    if (_info.iterations % 1000 == 0)
+    this->dataPtr->lastMeasurementTime = _info.simTime;
+    this->dataPtr->hasMeasurement = true;
+
+    // The pressure model uses kPa internally, while both Gazebo and ROS
+    // FluidPressure messages require Pa (and Pa^2 for variance).
+    constexpr double kPaToPa = 1000.0;
+    const double pressurePa = this->dataPtr->pressure * kPaToPa;
+    const double variancePa2 =
+      this->dataPtr->noiseSigma * this->dataPtr->noiseSigma * kPaToPa * kPaToPa;
+
+    // Publishing Sea_Pressure and depth estimate on gazebo topic
+    gz::msgs::FluidPressure gzPressureMsg;
+    gzPressureMsg.set_pressure(pressurePa);
+    gzPressureMsg.set_variance(variancePa2);
+
+    // Publishing the pressure message
+    this->dataPtr->gz_pressure_sensor_pub.Publish(gzPressureMsg);
+
+    // Publishing Sea_Pressure on Ros Topic
+    sensor_msgs::msg::FluidPressure rosPressureMsg;
+    rosPressureMsg.header.stamp.sec =
+      std::chrono::duration_cast<std::chrono::seconds>(_info.simTime).count();  // Time in seconds
+    rosPressureMsg.header.stamp.nanosec =
+      std::chrono::duration_cast<std::chrono::nanoseconds>(_info.simTime).count() %
+      1000000000;  // Time in nanoseconds
+    rosPressureMsg.fluid_pressure = pressurePa;
+    rosPressureMsg.variance = variancePa2;
+    this->dataPtr->ros_pressure_sensor_pub->publish(rosPressureMsg);
+
+    // publishing depth message
+    if (this->dataPtr->estimateDepth)
+    {
+      geometry_msgs::msg::PointStamped rosDepthMsg;
+      rosDepthMsg.point.z = this->dataPtr->inferredDepth;
+      rosDepthMsg.header.stamp.sec =
+        std::chrono::duration_cast<std::chrono::seconds>(this->dataPtr->lastMeasurementTime)
+          .count();
+      this->dataPtr->ros_depth_estimate_pub->publish(rosDepthMsg);
+    }
+
+    if (!_info.paused)
+    {
+      rclcpp::spin_some(this->rosNode);
+
+      if (_info.iterations % 1000 == 0)
+      {
+        gzmsg << "posim_ros_gz_plugins::SubseaPressureSensorPlugin::PostUpdate" << std::endl;
+      }
+    }
+  }
+  catch (const rclcpp::exceptions::RCLError &)
+  {
+    // The context may be invalidated between the check and publication/spin.
+    // Do not hide unrelated failures while ROS remains valid.
+    if (rclcpp::ok(context))
     {
-      gzmsg << "posim_ros_gz_plugins::SubseaPressureSensorPlugin::PostUpdate" << std::endl;
+      throw;
     }
   }
 }

gazebo/posim_gz_sensor_plugins/test/test_pressure_shutdown.cc

@@ -0,0 +1,40 @@
+#include <gtest/gtest.h>
+#include <chrono>
+#include <csignal>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/Pose.hh>
+#include <thread>
+#include "posim_gz_sensor_plugins/sea_pressure_sensor.hh"
+
+TEST(SeaPressure, PreservesSignalHandlerAndToleratesContextShutdown)
+{
+  auto handler = +[](int) {};
+  auto previous = std::signal(SIGINT, handler);
+  {
+    posim_gz_sensor_plugins::SubseaPressureSensorPlugin sensor;
+    gz::sim::EntityComponentManager ecm;
+    gz::sim::EventManager events;
+    auto entity = ecm.CreateEntity();
+    ecm.CreateComponent(entity, gz::sim::components::Name("pressure_shutdown_test"));
+    ecm.CreateComponent(entity, gz::sim::components::Pose(gz::math::Pose3d(0, 0, -5, 0, 0, 0)));
+    auto plugin = std::make_shared<sdf::Element>();
+    plugin->SetName("plugin");
+    auto ns = std::make_shared<sdf::Element>();
+    ns->SetName("namespace");
+    ns->AddValue("string", "pressure_shutdown_test", true);
+    plugin->InsertElement(ns);
+    sensor.Configure(entity, plugin, ecm, events);
+    EXPECT_EQ(std::signal(SIGINT, handler), handler);
+    gz::sim::UpdateInfo info;
+    info.simTime = std::chrono::milliseconds(125);
+    info.iterations = 1;
+    info.paused = false;
+    sensor.PreUpdate(info, ecm);
+    rclcpp::shutdown();
+    EXPECT_NO_THROW(sensor.PostUpdate(info, ecm));
+  }
+  std::signal(SIGINT, previous);
+}

gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc

@@ -102,7 +102,8 @@ void OceanCurrentPlugin::Configure(
 {
   if (!rclcpp::ok())
   {
-    rclcpp::init(0, nullptr);
+    // Gazebo owns process signal handling for embedded ROS nodes.
+    rclcpp::init(0, nullptr, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None);
     // gzerr << "ROS 2 has not been properly initialized. Please make sure you have initialized your
     // ROS 2 environment.";
   }
@@ -587,14 +588,41 @@ bool OceanCurrentPlugin::UpdateCurrentVertAngleModel(
 void OceanCurrentPlugin::PostUpdate(
   const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
 {
+  if (!this->dataPtr->rosNode)
+  {
+    return;
+  }
+  const auto context = this->dataPtr->rosNode->get_node_base_interface()->get_context();
+  if (!rclcpp::ok(context))
+  {
+    return;
+  }
   if (!_info.paused)
   {
-    rclcpp::spin_some(this->dataPtr->rosNode);
+    try
+    {
+      rclcpp::spin_some(this->dataPtr->rosNode);
+    }
+    catch (const rclcpp::exceptions::RCLError &)
+    {
+      // Another embedded ROS component may shut down the shared context
+      // after the check above. Do not hide errors during normal operation.
+      if (rclcpp::ok(context))
+      {
+        throw;
+      }
+      return;
+    }
     if (_info.iterations % 1000 == 0)
     {
       gzmsg << "posim_ros_gz_plugins::OceanCurrentPlugin::PostUpdate" << std::endl;
     }
   }
+  // A callback processed by spin_some may have shut down the context.
+  if (!rclcpp::ok(context))
+  {
+    return;
+  }
   auto * worldPlugin = posim_gz_world_plugins::OceanCurrentWorldPlugin::Instance();
   if (!worldPlugin)
   {
@@ -704,4 +732,4 @@ void OceanCurrentPlugin::PostUpdate(
 
 }  // namespace posim_ros_gz_plugins
 
-// #endif
+// #endif