diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 810f9a1..398ade0 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -395,7 +395,11 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element auto spin = [this]() { while (rclcpp::ok() && !impl_->stop_) { - impl_->executor_->spin_once(); + if (impl_->parent_model_->GetWorld()->IsPaused() == false) { + impl_->executor_->spin_once(); + } else { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } } }; impl_->thread_executor_spin_ = std::thread(spin);