Example: system_control_rate_timer_example

C++ example source code:

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file        |
   | See: https://www.mrpt.org/Authors - All rights reserved.                  |
   | Released under BSD License. See details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
   */

#include <mrpt/3rdparty/tclap/CmdLine.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/system/CControlledRateTimer.h>

#include <iostream>

// Declare the supported command line switches ===========
TCLAP::CmdLine cmd("system_control_rate_timer_example");

TCLAP::ValueArg<double> argRate1("1", "rate1", "rate1 (Hz)", false, 500.0, "rate (Hz)", cmd);
TCLAP::ValueArg<double> argRate2("2", "rate2", "rate2 (Hz)", false, 1500.0, "rate (Hz)", cmd);

void rateTest()
{
  const double TOTAL_EXECUTION_TIME = 5.0;  // seconds
  const double NOMINAL_RATE1 = argRate1.getValue();
  const double NOMINAL_RATE2 = argRate2.getValue();
  const unsigned int N = TOTAL_EXECUTION_TIME * NOMINAL_RATE1;
  const unsigned int STEP_TIME1 = N / 2;

  mrpt::system::CControlledRateTimer rate;
  rate.setRate(NOMINAL_RATE1);  // Hz

  mrpt::math::CVectorDouble estimatedRates, rawRates, actionRates, controlref;
  estimatedRates.resize(N);
  rawRates.resize(N);
  actionRates.resize(N);
  controlref.resize(N);

  printf("Running for %f seconds...\n", TOTAL_EXECUTION_TIME);

  for (unsigned int i = 0; i < N; i++)
  {
    // ---- here starts a regular user loop with a rate timer -----

    // Do the main loop job here:
    // ......

    // Wait so we start over exactly when the next period is about to start:
    rate.sleep();

    // ---- here ends a regular user loop with a rate timer -----

    // Graphs, for this example only, don't use in production! ;-)
    // Reference:
    const double desiredRate = (i < STEP_TIME1) ? NOMINAL_RATE1 : NOMINAL_RATE2;

    rate.setRate(desiredRate);

    estimatedRates[i] = rate.estimatedRate();
    rawRates[i] = rate.estimatedRateRaw();
    actionRates[i] = rate.actualControlledRate();
    controlref[i] = desiredRate;
  }

  mrpt::gui::CDisplayWindowPlots win(
      "Measured rates (Hz) [black] / estimated "
      "rates [red] / control action [blue]",
      600, 600);
  win.plot(rawRates, "k.3");
  win.hold_on();
  win.plot(estimatedRates, "r-");
  win.plot(actionRates, "b.2");
  win.plot(controlref, "m-");

  win.axis(-0.15 * N, N, -500, 2.0 * std::max(NOMINAL_RATE1, NOMINAL_RATE2));
  win.waitForKey();
}

int main(int argc, char** argv)
{
  try
  {
    // Parse arguments:
    if (!cmd.parse(argc, argv)) throw std::runtime_error("");  // should exit.

    rateTest();
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
    return -1;
  }
}