diff --git a/src/sim/sim.cpp b/src/sim/sim.cpp index 6fe3d888..03d8097c 100644 --- a/src/sim/sim.cpp +++ b/src/sim/sim.cpp @@ -16,6 +16,7 @@ void process_condition_callbacks(std::vector& cbs, mjtNum dt = time - cbs[i].last_call_timestamp; if (dt > cbs[i].seconds_between_calls) { cbs[i].last_return_value = cbs[i].cb(); + cbs[i].last_call_timestamp = time; } } } @@ -39,10 +40,11 @@ Config Sim::get_config() { return this->cfg; } void Sim::invoke_callbacks() { for (int i = 0; i < std::size(this->callbacks); ++i) { - Callback cb = this->callbacks[i]; + Callback& cb = this->callbacks[i]; mjtNum dt = this->d->time - cb.last_call_timestamp; if (dt > cb.seconds_between_calls) { cb.cb(); + cb.last_call_timestamp = this->d->time; } } } @@ -64,7 +66,7 @@ bool Sim::invoke_condition_callbacks() { void Sim::invoke_rendering_callbacks() { // bool scene_updated = false; for (size_t i = 0; i < std::size(this->rendering_callbacks); ++i) { - RenderingCallback cb = this->rendering_callbacks[i]; + RenderingCallback& cb = this->rendering_callbacks[i]; mjtNum dt = this->d->time - cb.last_call_timestamp; if (dt > cb.seconds_between_calls) { // if (!scene_updated) { @@ -75,6 +77,7 @@ void Sim::invoke_rendering_callbacks() { // } mjrContext* ctx = this->renderer.get_context(cb.id); cb.cb(cb.id, *ctx, this->renderer.scene, this->renderer.opt); + cb.last_call_timestamp = this->d->time; } } }