diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index df300c7267b4e6ff0bbf3f425392487fd5b90841..a3786baa3064ca9adea7dbffb4723111efe3c72b 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -18,6 +18,7 @@ class SensorBase; // std #include <memory> #include <map> +#include <chrono> namespace wolf { @@ -265,6 +266,14 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce unsigned int id() const; + // PROFILING + unsigned int n_capture_callback_; + unsigned int n_kf_callback_; + std::chrono::microseconds duration_; + std::chrono::time_point<std::chrono::high_resolution_clock> start_; + void startProfiling(); + void stopProfiling(); + protected: /** \brief process an incoming capture * @@ -384,6 +393,16 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const; }; +inline void ProcessorBase::startProfiling() +{ + start_ = std::chrono::high_resolution_clock::now(); +} + +inline void ProcessorBase::stopProfiling() +{ + duration_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_); +} + inline bool ProcessorBase::isVotingActive() const { return params_->voting_active; diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index a6759817ae22e8b38c5f5dbce2144e2d8aee23c5..c2dce560c5a701f2d5131c34c31fa489da96a4f2 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -70,6 +70,11 @@ class SolverManager FULL }; + // PROFILING + unsigned int n_solve_; + std::chrono::microseconds duration_manager_; + std::chrono::microseconds duration_solver_; + protected: ProblemPtr wolf_problem_; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index b151a6929ed93ecb4c231ffcd409df89ee3c36d1..b649656837b8a056a6962831c5f4e45cd83cd070 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -605,6 +605,9 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + // pause processor profiling + _processor_ptr->stopProfiling(); + for (auto sensor : hardware_ptr_->getSensorList()) for (auto processor : sensor->getProcessorList()) if (processor && (processor != _processor_ptr) ) @@ -629,6 +632,9 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro // notify tree manager if (tree_manager_) tree_manager_->keyFrameCallback(_keyframe_ptr); + + // resume processor profiling + _processor_ptr->startProfiling(); } bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index caa850ebcd7b3b6d9e443c05266cc1e1d67491c0..203041c1e88b45948a4ccc27fcd920d228ae0fde 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -37,6 +37,9 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _ assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame"); WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); + // profiling + n_kf_callback_++; + startProfiling(); // asking if key frame should be stored if (storeKeyFrame(_keyframe_ptr)) @@ -46,6 +49,8 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _ if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other)) processKeyFrame(_keyframe_ptr, _time_tol_other); + // profiling + stopProfiling(); } void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) @@ -53,6 +58,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture"); WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp()); + // profiling + n_capture_callback_++; + startProfiling(); + // apply prior in problem if not done (very first capture) if (getProblem() && !getProblem()->isPriorSet()) getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp()); @@ -64,6 +73,9 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) // if trigger, process directly without buffering if (triggerInCapture(_capture_ptr)) processCapture(_capture_ptr); + + // profiling + stopProfiling(); } void ProcessorBase::remove() diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 4994631c38737949c6aef75b2cada0cd810c0d96..7d9a75c49adb41ab1698b599157ea4fd737c17ec 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -12,6 +12,7 @@ SolverManager::SolverManager(const ProblemPtr& _problem) : SolverManager::SolverManager(const ProblemPtr& _problem, const ParamsSolverPtr& _params) : + n_solve_(0), wolf_problem_(_problem), params_(_params) { @@ -132,11 +133,16 @@ std::string SolverManager::solve() std::string SolverManager::solve(const ReportVerbosity report_level) { + auto start = std::chrono::high_resolution_clock::now(); + n_solve_++; + // update problem update(); // call derived solver + auto start_derived = std::chrono::high_resolution_clock::now(); std::string report = solveDerived(report_level); + duration_solver_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_derived); // update StateBlocks with optimized state value. /// @todo whatif someone has changed the state notification during opti ?? @@ -151,6 +157,8 @@ std::string SolverManager::solve(const ReportVerbosity report_level) stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_ } + duration_manager_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start); + return report; }