wip

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;

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_;

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

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()

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;
 }