perf: reduces CPU load by sleeping in paused state

CHANGELOG.md

@@ -17,13 +17,15 @@ Loading and reset times are reported in the server debug log. All plugin stats c
 * When loading takes more than 0.25 seconds the simulation is no longer automatically paused.
 * Fixed fetching of body quaternion in `get_body_state` service.
 * *tests*: PendulumEnvFixture now makes sure `mj_forward` has been run at least once. This ensures the data object is populated with correct initial positions and velocities.
-* re-added services for getting and setting gravity, that somehow vanished.
-* fixed flaky tests that did not consider control callbacks being called in paused mode, too (#40).
+* Re-added services for getting and setting gravity, that somehow vanished.
+* Fixed flaky tests that did not consider control callbacks being called in paused mode, too (#40).
+* Fixed bug that would not allow breaking out of *as fast as possible* stepping in headless mode without shutting down the simulation.
 
 ### Changed
 * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).
 * Increased test coverage of `mujoco_ros_sensors` plugin.
 * Split monolithic ros interface tests into more individual tests.
+* Added sleeping at least until the next lowerbound GUI refresh when paused to reduce cpu load.
 
 Contributors: @DavidPL1
 

mujoco_ros/src/physics.cpp

@@ -93,9 +93,9 @@ void MujocoEnv::physicsLoop()
 
 void MujocoEnv::simPausedPhysics(mjtNum &syncSim)
 {
+	const auto startCPU = Clock::now();
 	if (settings_.env_steps_request.load() > 0) { // Action call or arrow keys used for stepping
-		syncSim             = data_->time;
-		const auto startCPU = Clock::now();
+		syncSim = data_->time;
 
 		while (settings_.env_steps_request.load() > 0 &&
 		       (connected_viewers_.empty() ||
@@ -131,6 +131,9 @@ void MujocoEnv::simPausedPhysics(mjtNum &syncSim)
 		// Run mj_forward, to update rendering and joint sliders
 		mj_forward(model_.get(), data_.get());
 		publishSimTime(data_->time);
+		// Sleep for the difference between the lower bound render rate (30Hz) and the time it took to run the forward
+		// step to reduce cpu load
+		std::this_thread::sleep_for(Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_) - (Clock::now() - startCPU));
 	}
 }
 
@@ -194,7 +197,7 @@ void MujocoEnv::simUnpausedPhysics(mjtNum &syncSim, std::chrono::time_point<Cloc
 		       (Clock::now() - startCPU < Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_) ||
 		        connected_viewers_.empty()) && // only break if rendering UI is actually necessary
 		       !settings_.exit_request.load() &&
-		       num_steps_until_exit_ != 0) {
+		       num_steps_until_exit_ != 0 && settings_.run.load()) {
 			// measure slowdown before first step
 			if (!measured && elapsedSim) {
 				if (settings_.real_time_index != 0) {