diff --git a/gazebo/posim_gz_sensor_plugins/CMakeLists.txt b/gazebo/posim_gz_sensor_plugins/CMakeLists.txt index 9c87f497..e5855d10 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_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() diff --git a/gazebo/posim_gz_sensor_plugins/src/UsblTransceiver.cc b/gazebo/posim_gz_sensor_plugins/src/UsblTransceiver.cc index bdb323d5..c8b53ec9 100644 --- a/gazebo/posim_gz_sensor_plugins/src/UsblTransceiver.cc +++ b/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("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(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(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 diff --git a/gazebo/posim_gz_sensor_plugins/src/UsblTransponder.cc b/gazebo/posim_gz_sensor_plugins/src/UsblTransponder.cc index 8fc544ca..5cdc8788 100644 --- a/gazebo/posim_gz_sensor_plugins/src/UsblTransponder.cc +++ b/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 diff --git a/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc index a0e132bc..5c932dfc 100644 --- a/gazebo/posim_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/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::duration(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(_info.simTime).count(); // Time in seconds - rosPressureMsg.header.stamp.nanosec = - std::chrono::duration_cast(_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(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::duration(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(_info.simTime).count(); // Time in seconds + rosPressureMsg.header.stamp.nanosec = + std::chrono::duration_cast(_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(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; } } } diff --git a/gazebo/posim_gz_sensor_plugins/test/test_pressure_shutdown.cc b/gazebo/posim_gz_sensor_plugins/test/test_pressure_shutdown.cc new file mode 100644 index 00000000..578ea8db --- /dev/null +++ b/gazebo/posim_gz_sensor_plugins/test/test_pressure_shutdown.cc @@ -0,0 +1,40 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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(); + plugin->SetName("plugin"); + auto ns = std::make_shared(); + 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); +} diff --git a/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc b/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc index 0f627138..c541da8a 100644 --- a/gazebo/posim_ros_gz_plugins/src/OceanCurrentPlugin.cc +++ b/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 \ No newline at end of file +// #endif diff --git a/gazebo/posim_ros_gz_plugins/src/SphericalCoords.cc b/gazebo/posim_ros_gz_plugins/src/SphericalCoords.cc index e8e9a76f..15d184a1 100644 --- a/gazebo/posim_ros_gz_plugins/src/SphericalCoords.cc +++ b/gazebo/posim_ros_gz_plugins/src/SphericalCoords.cc @@ -55,7 +55,8 @@ void SphericalCoords::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); } this->ros_node_ = std::make_shared("sc_node"); @@ -225,9 +226,31 @@ bool SphericalCoords::SetOriginSphericalCoord( void SphericalCoords::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 + // after the check above. Do not hide errors during normal operation. + if (rclcpp::ok(context)) + { + throw; + } + return; + } if (_info.iterations % 1000 == 0) {