From a86fc059cc5a6cc4491b064ccd844042c90a2e62 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 30 Sep 2024 19:28:42 +0000 Subject: [PATCH 1/2] Just don't spin the executor if the sim is paused --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 810f9a1..4dfa9db 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -395,7 +395,9 @@ 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(); + } } }; impl_->thread_executor_spin_ = std::thread(spin); From 944f12f887eeb7e9809915a49973f5a11f283c5b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 1 Oct 2024 06:33:39 +0000 Subject: [PATCH 2/2] Add some sleep in the while loop --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 4dfa9db..398ade0 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -397,6 +397,8 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element while (rclcpp::ok() && !impl_->stop_) { if (impl_->parent_model_->GetWorld()->IsPaused() == false) { impl_->executor_->spin_once(); + } else { + std::this_thread::sleep_for(std::chrono::microseconds(100)); } } };