From f640ba0fe10b945c8b68e5c8a508c418c91dd553 Mon Sep 17 00:00:00 2001 From: David Leins Date: Tue, 19 Nov 2024 18:41:26 +0100 Subject: [PATCH] perf: reduces CPU load by sleeping in paused state --- CHANGELOG.md | 6 ++++-- mujoco_ros/src/physics.cpp | 9 ++++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6f3e134c..a43364c8 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/mujoco_ros/src/physics.cpp b/mujoco_ros/src/physics.cpp index c0140e51..b225d637 100644 --- a/mujoco_ros/src/physics.cpp +++ b/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