diff --git a/common/config/config.json b/common/config/config.json index 80d6589a63910e414ff02d2db35e46291eb69130..4c0ec32253deeac8ac77c89a1f6659f8f437ee2d 100644 --- a/common/config/config.json +++ b/common/config/config.json @@ -9,7 +9,8 @@ "flip": false, "nostereo": false, "scale": 1.0, - "flip_vert": false + "flip_vert": false, + "max_fps": 25 }, "calibrate": false, "calibration": { diff --git a/vision/include/ftl/local.hpp b/vision/include/ftl/local.hpp index cf18b9928ac8d0e4e13479cced920ae3038b0df2..80b527cc74c50122274f9568c1ce8b5b9ad2b653 100644 --- a/vision/include/ftl/local.hpp +++ b/vision/include/ftl/local.hpp @@ -28,6 +28,7 @@ class LocalSource { private: double timestamp_; + double tps_; bool stereo_; //float fps_; bool flip_; diff --git a/vision/src/local.cpp b/vision/src/local.cpp index ea000e1a07c6550fc5085db59ed6106fe80fb181..d0e542669df9e2e6faba2ba0268fb53b7d99183a 100644 --- a/vision/src/local.cpp +++ b/vision/src/local.cpp @@ -6,6 +6,7 @@ #include <string> #include <chrono> +#include <thread> #include <ftl/local.hpp> #include <opencv2/core.hpp> @@ -19,6 +20,8 @@ using std::string; using std::chrono::duration_cast; using std::chrono::duration; using std::chrono::high_resolution_clock; +using std::chrono::milliseconds; +using std::this_thread::sleep_for; LocalSource::LocalSource(nlohmann::json &config) : timestamp_(0.0), @@ -51,6 +54,8 @@ LocalSource::LocalSource(nlohmann::json &config) } else { stereo_ = true; } + + tps_ = 1.0 / (double)config["max_fps"]; } LocalSource::LocalSource(const string &vid, nlohmann::json &config) @@ -89,6 +94,8 @@ LocalSource::LocalSource(const string &vid, nlohmann::json &config) LOG(INFO) << "Video size : " << frame.cols << "x" << frame.rows; stereo_ = false; } + + tps_ = 1.0 / (double)config["max_fps"]; } bool LocalSource::left(cv::Mat &l) { @@ -177,8 +184,15 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) { } // Record timestamp - timestamp_ = duration_cast<duration<double>>( + double timestamp = duration_cast<duration<double>>( high_resolution_clock::now().time_since_epoch()).count(); + + // Limit max framerate + if (timestamp - timestamp_ < tps_) { + sleep_for(milliseconds((int)std::round((tps_ - (timestamp - timestamp_))*1000))); + } + + timestamp_ = timestamp; if (camera_b_ || !stereo_) { if (!camera_a_->retrieve(l)) {