MRPT  2.0.4
test.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in https://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 
11 #include <mrpt/3rdparty/tclap/CmdLine.h>
14 #include <iostream>
15 
16 // Declare the supported command line switches ===========
17 TCLAP::CmdLine cmd("system_control_rate_timer_example");
18 
19 TCLAP::ValueArg<double> argRate1(
20  "1", "rate1", "rate1 (Hz)", false, 500.0, "rate (Hz)", cmd);
21 TCLAP::ValueArg<double> argRate2(
22  "2", "rate2", "rate2 (Hz)", false, 1500.0, "rate (Hz)", cmd);
23 
24 void rateTest()
25 {
26  const double TOTAL_EXECUTION_TIME = 5.0; // seconds
27  const double NOMINAL_RATE1 = argRate1.getValue();
28  const double NOMINAL_RATE2 = argRate2.getValue();
29  const unsigned int N = TOTAL_EXECUTION_TIME * NOMINAL_RATE1;
30  const unsigned int STEP_TIME1 = N / 2;
31 
33  rate.setRate(NOMINAL_RATE1); // Hz
34 
35  mrpt::math::CVectorDouble estimatedRates, rawRates, actionRates, controlref;
36  estimatedRates.resize(N);
37  rawRates.resize(N);
38  actionRates.resize(N);
39  controlref.resize(N);
40 
41  printf("Running for %f seconds...\n", TOTAL_EXECUTION_TIME);
42 
43  for (unsigned int i = 0; i < N; i++)
44  {
45  // ---- here starts a regular user loop with a rate timer -----
46 
47  // Do the main loop job here:
48  // ......
49 
50  // Wait so we start over exactly when the next period is about to start:
51  rate.sleep();
52 
53  // ---- here ends a regular user loop with a rate timer -----
54 
55  // Graphs, for this example only, don't use in production! ;-)
56  // Reference:
57  const double desiredRate =
58  (i < STEP_TIME1) ? NOMINAL_RATE1 : NOMINAL_RATE2;
59 
60  rate.setRate(desiredRate);
61 
62  estimatedRates[i] = rate.estimatedRate();
63  rawRates[i] = rate.estimatedRateRaw();
64  actionRates[i] = rate.actualControlledRate();
65  controlref[i] = desiredRate;
66  }
67 
69  "Measured rates (Hz) [black] / estimated "
70  "rates [red] / control action [blue]",
71  600, 600);
72  win.plot(rawRates, "k.3");
73  win.hold_on();
74  win.plot(estimatedRates, "r-");
75  win.plot(actionRates, "b.2");
76  win.plot(controlref, "m-");
77 
78  win.axis(-0.15 * N, N, -500, 2.0 * std::max(NOMINAL_RATE1, NOMINAL_RATE2));
79  win.waitForKey();
80 }
81 
82 int main(int argc, char** argv)
83 {
84  try
85  {
86  // Parse arguments:
87  if (!cmd.parse(argc, argv))
88  throw std::runtime_error(""); // should exit.
89 
90  rateTest();
91  return 0;
92  }
93  catch (const std::exception& e)
94  {
95  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
96  return -1;
97  }
98 }
CControlledRateTimer.h
mrpt::system::CControlledRateTimer
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
Definition: CControlledRateTimer.h:47
mrpt::math::CVectorDynamic::resize
void resize(std::size_t N, bool zeroNewElements=false)
Definition: CVectorDynamic.h:151
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
argRate2
TCLAP::ValueArg< double > argRate2("2", "rate2", "rate2 (Hz)", false, 1500.0, "rate (Hz)", cmd)
cmd
TCLAP::CmdLine cmd("system_control_rate_timer_example")
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:50
mrpt::system::CControlledRateTimer::setRate
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
Definition: CControlledRateTimer.cpp:26
mrpt::math::CVectorDynamic< double >
mrpt::gui::CDisplayWindowPlots
Create a GUI window and display plots with MATLAB-like interfaces and commands.
Definition: CDisplayWindowPlots.h:33
mrpt::system::CControlledRateTimer::actualControlledRate
double actualControlledRate() const
Gets the actual controller output: the rate (Hz) of the internal CRateTimer object.
Definition: CControlledRateTimer.h:116
mrpt::system::CControlledRateTimer::sleep
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CControlledRateTimer.cpp:37
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:51
mrpt::system::CControlledRateTimer::estimatedRateRaw
double estimatedRateRaw() const
Last actual execution rate measured (Hz), without low-pass filtering.
Definition: CControlledRateTimer.h:123
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
CDisplayWindowPlots.h
mrpt::system::CControlledRateTimer::estimatedRate
double estimatedRate() const
Gets the latest estimated run rate (Hz), which comes from actual period measurement,...
Definition: CControlledRateTimer.h:120
rateTest
void rateTest()
Definition: vision_stereo_rectify/test.cpp:24
argRate1
TCLAP::ValueArg< double > argRate1("1", "rate1", "rate1 (Hz)", false, 500.0, "rate (Hz)", cmd)



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020