Commit d438243f authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add warning when period is exceeded.

parent 98edaef4
......@@ -114,13 +114,15 @@ void Log::start_it() {
timeorigin_;
}
void Log::stop_it() {
double Log::stop_it() {
struct timeval current;
gettimeofday(&current, 0);
time_stop_it_ =
((double)current.tv_sec + 0.000001 * (double)current.tv_usec) -
timeorigin_;
return time_stop_it_ - time_start_it_;
}
void Log::save(std::string &fileName) {
......
......@@ -92,7 +92,8 @@ public:
void save(std::string &fileName);
void start_it();
void stop_it();
/// \return the elapsed time since the previous \ref start_it
double stop_it();
};
} // namespace rc_sot_system
......
......@@ -967,7 +967,11 @@ void RCSotController::one_iteration() {
readControl(controlValues_);
// Chrono stop.
RcSotLog_.stop_it();
double it_duration = RcSotLog_.stop_it();
if (it_duration > dt_) {
ROS_WARN_THROTTLE(1, "Iteration duration (%d) bigger than max period (%d)",
it_duration, dt_);
}
/// Store everything in Log.
RcSotLog_.record(DataOneIter_);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment