diff --git a/gazebo/dave_ros_gz_plugins/src/DVLBridge.cc b/gazebo/dave_ros_gz_plugins/src/DVLBridge.cc index 66b98204..11709821 100644 --- a/gazebo/dave_ros_gz_plugins/src/DVLBridge.cc +++ b/gazebo/dave_ros_gz_plugins/src/DVLBridge.cc @@ -183,14 +183,30 @@ void DVLBridge::receiveGazeboCallback(const gz::msgs::DVLVelocityTracking & msg) void DVLBridge::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - if (!_info.paused) + if (_info.paused || !this->ros_node_ || !rclcpp::ok()) { - rclcpp::spin_some(this->ros_node_); + return; + } - if (_info.iterations % 1000 == 0) + try + { + rclcpp::spin_some(this->ros_node_); + } + catch (const rclcpp::exceptions::RCLError & error) + { + // SIGINT can invalidate the global context between the ok() check and + // temporary executor construction inside rclcpp::spin_some(). + if (rclcpp::ok()) { - gzmsg << "dave_ros_gz_plugins::DVLBridge::PostUpdate" << std::endl; + throw; } + gzdbg << "Skipping DVL ROS spin during shutdown: " << error.what() << std::endl; + return; + } + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_ros_gz_plugins::DVLBridge::PostUpdate" << std::endl; } }