MRPT
2.0.4
|
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make the loop run at the desired rate (in Hz).
This class implements a PI controller on top of a vanilla CRateTimer object, ensuring a high accuracy in achieved execution rates. Note that this is done by setting a slightly-higher rate ("control action") to the internal CRateTimer, such that the error between the user-provided expected rate and the actual measured rate (low-pass filtered) is decreased by means of a PI controller.
Note that rates higher than a few kHz are not attainable in all CPUs and/or kernel versions. Find below some graphs illustrating how this class tries to achieve a constant setpoint rate (given by the user), reacting to changes in the setpoint values:
This graphs is generated with the example:
system_control_rate_timer_example --rate1 2000.0 --rate2 4000.0
All the parameters for the PI controller and low-pass filter (rate estimator) are settable by the user to adapt them to specific needs.
Definition at line 47 of file CControlledRateTimer.h.
#include <mrpt/system/CControlledRateTimer.h>
Public Member Functions | |
Main API | |
CControlledRateTimer (const double rate_hz=1.0) | |
Ctor: specifies the desired rate (Hz) More... | |
virtual | ~CControlledRateTimer ()=default |
Dtor. More... | |
void | setRate (const double rate_hz) |
Changes the object loop rate (Hz) More... | |
bool | sleep () |
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the previous call. More... | |
PI control parameters | |
double | controllerParam_Kp () const |
PI controller Kp parameter [default=1.0]. More... | |
void | controllerParam_Kp (double v) |
double | controllerParam_Ti () const |
PI controller Ti parameter [default=0.0194]. More... | |
void | controllerParam_Ti (double v) |
double | lowPassParam_a0 () const |
Low-pass filter a0 value [default=0.9]: estimation = a0*input + (1-a0)*former_estimation. More... | |
void | lowPassParam_a0 (double v) |
double | followErrorRatioToRaiseWarning () const |
Get/set ratio threshold for issuing a warning (via COutputLogger interface) if the achieved rate is not this close to the set-point [Default=0.2, =20%]. More... | |
void | followErrorRatioToRaiseWarning (double v) |
double | actualControlledRate () const |
Gets the actual controller output: the rate (Hz) of the internal CRateTimer object. More... | |
double | estimatedRate () const |
Gets the latest estimated run rate (Hz), which comes from actual period measurement, low-pass filtered. More... | |
double | estimatedRateRaw () const |
Last actual execution rate measured (Hz), without low-pass filtering. More... | |
Static Public Member Functions | |
static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_colors () |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More... | |
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_names () |
Map from VerbosityLevels to their corresponding names. More... | |
Protected Attributes | |
VerbosityLevel | m_min_verbosity_level {LVL_INFO} |
Provided messages with VerbosityLevel smaller than this value shall be ignored. More... | |
Private Member Functions | |
bool | internalUpdateRateEstimate () |
Private Attributes | |
double | m_rate_hz = 1.0 |
mrpt::system::CRateTimer | m_ratetimer |
the one control acts on More... | |
double | m_lowPass_a0 = 0.99 |
double | m_Kp = 1.0 |
double | m_Ti = 0.1 |
double | m_followErrorRatioForWarning = 0.20 |
double | m_lastControlError = .0 |
double | m_currentEstimatedRate = 1.0 |
double | m_lastRawRate = 1.0 |
double | m_lastTic = 0 |
mrpt::system::CTicTac | m_tic |
Logging methods | |
void | logStr (const VerbosityLevel level, std::string_view msg_str) const |
Main method to add the specified message string to the logger. More... | |
void | logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3 |
Alternative logging method, which mimics the printf behavior. More... | |
void void | logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const |
Log the given message only if the condition is satisfied. More... | |
void | setLoggerName (const std::string &name) |
Set the name of the COutputLogger instance. More... | |
std::string | getLoggerName () const |
Return the name of the COutputLogger instance. More... | |
void | setMinLoggingLevel (const VerbosityLevel level) |
Set the minimum logging level for which the incoming logs are going to be taken into account. More... | |
void | setVerbosityLevel (const VerbosityLevel level) |
alias of setMinLoggingLevel() More... | |
VerbosityLevel | getMinLoggingLevel () const |
bool | isLoggingLevelVisible (VerbosityLevel level) const |
void | getLogAsString (std::string &log_contents) const |
Fill the provided string with the contents of the logger's history in std::string representation. More... | |
std::string | getLogAsString () const |
Get the history of COutputLogger instance in a string representation. More... | |
void | writeLogToFile (const std::string *fname_in=nullptr) const |
Write the contents of the COutputLogger instance to an external file. More... | |
void | dumpLogToConsole () const |
Dump the current contents of the COutputLogger instance in the terminal window. More... | |
std::string | getLoggerLastMsg () const |
Return the last Tmsg instance registered in the logger history. More... | |
void | getLoggerLastMsg (std::string &msg_str) const |
Fill inputtted string with the contents of the last message in history. More... | |
void | loggerReset () |
Reset the contents of the logger instance. More... | |
void | logRegisterCallback (output_logger_callback_t userFunc) |
bool | logDeregisterCallback (output_logger_callback_t userFunc) |
bool | logging_enable_console_output {true} |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More... | |
bool | logging_enable_keep_record {false} |
[Default=false] Enables storing all messages into an internal list. More... | |
CControlledRateTimer::CControlledRateTimer | ( | const double | rate_hz = 1.0 | ) |
Ctor: specifies the desired rate (Hz)
Definition at line 20 of file CControlledRateTimer.cpp.
References m_tic, setRate(), and mrpt::system::CTicTac::Tic().
|
virtualdefault |
Dtor.
|
inline |
Gets the actual controller output: the rate (Hz) of the internal CRateTimer object.
Definition at line 116 of file CControlledRateTimer.h.
References m_ratetimer, and mrpt::system::CRateTimer::rate().
|
inline |
PI controller Kp parameter [default=1.0].
Definition at line 74 of file CControlledRateTimer.h.
References m_Kp.
|
inline |
Definition at line 75 of file CControlledRateTimer.h.
References ASSERT_ABOVE_, and m_Kp.
|
inline |
PI controller Ti parameter [default=0.0194].
Definition at line 82 of file CControlledRateTimer.h.
References m_Ti.
|
inline |
Definition at line 83 of file CControlledRateTimer.h.
References ASSERT_ABOVEEQ_, and m_Ti.
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 190 of file COutputLogger.cpp.
|
inline |
Gets the latest estimated run rate (Hz), which comes from actual period measurement, low-pass filtered.
Definition at line 120 of file CControlledRateTimer.h.
References m_currentEstimatedRate.
|
inline |
Last actual execution rate measured (Hz), without low-pass filtering.
Definition at line 123 of file CControlledRateTimer.h.
References m_lastRawRate.
|
inline |
Get/set ratio threshold for issuing a warning (via COutputLogger interface) if the achieved rate is not this close to the set-point [Default=0.2, =20%].
Definition at line 103 of file CControlledRateTimer.h.
References m_followErrorRatioForWarning.
|
inline |
Definition at line 107 of file CControlledRateTimer.h.
References ASSERT_ABOVE_, ASSERT_BELOWEQ_, and m_followErrorRatioForWarning.
|
inherited |
Fill the provided string with the contents of the logger's history in std::string representation.
Definition at line 154 of file COutputLogger.cpp.
|
inherited |
Get the history of COutputLogger instance in a string representation.
Definition at line 159 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
inherited |
Return the last Tmsg instance registered in the logger history.
Definition at line 195 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Fill inputtted string with the contents of the last message in history.
Definition at line 201 of file COutputLogger.cpp.
|
inherited |
Return the name of the COutputLogger instance.
Definition at line 143 of file COutputLogger.cpp.
|
inlineinherited |
Definition at line 201 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::apps::RawlogGrabberApp::dump_verbose_info(), mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
private |
Definition at line 80 of file CControlledRateTimer.cpp.
References m_currentEstimatedRate, m_lastRawRate, m_lastTic, m_lowPass_a0, m_tic, and mrpt::system::CTicTac::Tac().
Referenced by sleep().
|
inlineinherited |
Definition at line 202 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
|
inherited |
Log the given message only if the condition is satisfied.
Definition at line 131 of file COutputLogger.cpp.
|
inherited |
Definition at line 291 of file COutputLogger.cpp.
References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Alternative logging method, which mimics the printf behavior.
Handy for not having to first use mrpt::format to pass a std::string message to logStr
Definition at line 91 of file COutputLogger.cpp.
Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::hmtslam::CHMTSLAM::thread_3D_viewer(), mrpt::hmtslam::CHMTSLAM::thread_LSLAM(), mrpt::hmtslam::CHMTSLAM::thread_TBI(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().
|
inherited |
Reset the contents of the logger instance.
Called upon construction.
Definition at line 206 of file COutputLogger.cpp.
References mrpt::system::LVL_INFO.
|
staticinherited |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.
Handy for coloring the input based on the verbosity of the message
Definition at line 47 of file COutputLogger.cpp.
References logging_levels_to_colors.
Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().
|
staticinherited |
Map from VerbosityLevels to their corresponding names.
Handy for printing the current message VerbosityLevel along with the actual content
Definition at line 60 of file COutputLogger.cpp.
References logging_levels_to_names.
Referenced by mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Definition at line 278 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Main method to add the specified message string to the logger.
Definition at line 72 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.
Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().
|
inline |
Low-pass filter a0 value [default=0.9]: estimation = a0*input + (1-a0)*former_estimation.
Definition at line 91 of file CControlledRateTimer.h.
References m_lowPass_a0.
|
inline |
Definition at line 92 of file CControlledRateTimer.h.
References ASSERT_ABOVE_, ASSERT_BELOWEQ_, and m_lowPass_a0.
|
inherited |
Set the name of the COutputLogger instance.
Definition at line 138 of file COutputLogger.cpp.
Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base(), mrpt::apps::ICP_SLAM_App_Live::ICP_SLAM_App_Live(), mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog(), mrpt::graphslam::CWindowManager::initCWindowManager(), mrpt::apps::MonteCarloLocalization_Base::MonteCarloLocalization_Base(), mrpt::apps::RBPF_SLAM_App_Base::RBPF_SLAM_App_Base(), and mrpt::apps::RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog().
|
inherited |
Set the minimum logging level for which the incoming logs are going to be taken into account.
String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance
Definition at line 144 of file COutputLogger.cpp.
Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
void CControlledRateTimer::setRate | ( | const double | rate_hz | ) |
Changes the object loop rate (Hz)
Definition at line 26 of file CControlledRateTimer.cpp.
References ASSERT_ABOVE_, m_currentEstimatedRate, m_lastControlError, m_lastTic, and m_rate_hz.
Referenced by CControlledRateTimer().
|
inherited |
alias of setMinLoggingLevel()
Definition at line 149 of file COutputLogger.cpp.
Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::math::ransac_detect_2D_lines(), and mrpt::apps::ICP_SLAM_App_Base::run().
bool CControlledRateTimer::sleep | ( | ) |
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the previous call.
Definition at line 37 of file CControlledRateTimer.cpp.
References internalUpdateRateEstimate(), m_currentEstimatedRate, m_followErrorRatioForWarning, m_Kp, m_lastControlError, m_rate_hz, m_ratetimer, m_Ti, MRPT_LOG_THROTTLE_WARN_FMT, mrpt::system::CRateTimer::rate(), mrpt::saturate_val(), mrpt::system::CRateTimer::setRate(), and mrpt::system::CRateTimer::sleep().
|
inherited |
Write the contents of the COutputLogger instance to an external file.
Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided
Definition at line 165 of file COutputLogger.cpp.
References ASSERTMSG_, and mrpt::format().
|
inherited |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.
Definition at line 240 of file system/COutputLogger.h.
|
inherited |
[Default=false] Enables storing all messages into an internal list.
Definition at line 243 of file system/COutputLogger.h.
|
private |
Definition at line 138 of file CControlledRateTimer.h.
Referenced by estimatedRate(), internalUpdateRateEstimate(), setRate(), and sleep().
|
private |
Definition at line 134 of file CControlledRateTimer.h.
Referenced by followErrorRatioToRaiseWarning(), and sleep().
|
private |
Definition at line 131 of file CControlledRateTimer.h.
Referenced by controllerParam_Kp(), and sleep().
|
private |
Definition at line 135 of file CControlledRateTimer.h.
|
private |
Definition at line 138 of file CControlledRateTimer.h.
Referenced by estimatedRateRaw(), and internalUpdateRateEstimate().
|
private |
Definition at line 139 of file CControlledRateTimer.h.
Referenced by internalUpdateRateEstimate(), and setRate().
|
private |
Definition at line 130 of file CControlledRateTimer.h.
Referenced by internalUpdateRateEstimate(), and lowPassParam_a0().
|
protectedinherited |
Provided messages with VerbosityLevel smaller than this value shall be ignored.
Definition at line 253 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().
|
private |
Definition at line 127 of file CControlledRateTimer.h.
|
private |
the one control acts on
Definition at line 128 of file CControlledRateTimer.h.
Referenced by actualControlledRate(), and sleep().
|
private |
Definition at line 132 of file CControlledRateTimer.h.
Referenced by controllerParam_Ti(), and sleep().
|
private |
Definition at line 140 of file CControlledRateTimer.h.
Referenced by CControlledRateTimer(), and internalUpdateRateEstimate().
Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020 |