Fix priority problem when logging + add CPU load introspection.

CMakeLists.txt

@@ -41,6 +41,7 @@ SET(PKG_CONFIG_EXTRA "plugindir=${PLUGINDIR}")
 PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 
 # Search for Boost.
+SET(BOOST_COMPONENTS serialization system unit_test_framework thread)
 SEARCH_FOR_BOOST()
 SEARCH_FOR_PTHREAD()
 

include/dynamic-graph/process-list.hh

+/* Copyright LAAS, CNRS
+ * Author: O. Stasse, 2019
+ * See LICENSE file in the root directory of this repository.
+ */
+
+#ifndef DYNAMIC_GRAPH_PROCESS_LIST_H_
+# define DYNAMIC_GRAPH_PROCESS_LIST_H
+
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/serialization/vector.hpp>
+
+# include <dynamic-graph/fwd.hh>
+# include <dynamic-graph/dynamic-graph-api.h>
+
+namespace dynamicgraph
+{
+  namespace CPU
+  {
+    class DYNAMIC_GRAPH_DLLAPI ProcessData
+    {
+    };
+    
+    class DYNAMIC_GRAPH_DLLAPI ProcessList
+    {
+    public:
+      ProcessList();
+      
+    };
+    
+    /// \class This class gather information on a specific CPU.
+    /// 
+    class DYNAMIC_GRAPH_DLLAPI CPUData
+    {
+    public:
+      
+      CPUData();
+      int cpu_id_;
+      
+      inline unsigned long long int
+	computePeriod(unsigned long long int &a,
+		      unsigned long long int &b)
+      {
+	return (a>b) ? a-b :0;
+      }
+      
+      /// \brief Various classes of time spend by the CPU
+      /// @{
+      /// Total time
+      unsigned long long int total_time_;
+      /// Time spend in user mode
+      unsigned long long int user_mode_time_;
+      /// Time spend in user mode with low priority (nice mode)
+      unsigned long long int nice_time_;
+      /// Time spend in system mode
+      unsigned long long int system_time_;
+      /// Time spend in system mode
+      unsigned long long int system_all_time_;
+      /// Time spend in doing nothing.
+      unsigned long long int idle_time_;
+      /// Time spend in doing nothing.
+      unsigned long long int idle_all_time_;      
+      /// Time spend in waiting an input/output to complete.
+      unsigned long long int iowait_time_;
+      /// Time spend in servicing hardware interrupts.
+      unsigned long long int irq_time_;
+      /// Time spend in servicing software interrupts.
+      unsigned long long int softirq_time_;
+      /// Time spend in other operating systems in a virtualized environments
+      /// Never doing this for control !
+      unsigned long long int steal_time_;
+      /// Time spend running a virtual CPU for guest operating systems
+      /// under the control of the Linux kernel
+      unsigned long long int guest_time_;
+      /// Time spent running a niced guest
+      /// (virtual CPU for guest operating systems under the
+      /// control of the Linux kernel)
+      unsigned long long int guest_nice_time_;
+      /// @}
+
+      /// \brief Various classes of time spend by the CPU by period
+      /// @{
+      /// Total time
+      unsigned long long int total_period_;
+      /// Time spend in user mode
+      unsigned long long int user_mode_period_;
+      /// Time spend in user mode with low priority (nice mode)
+      unsigned long long int nice_period_;
+      /// Time spend in system mode
+      unsigned long long int system_period_;
+      /// Time spend in all system mode
+      unsigned long long int system_all_period_;
+      /// Time spend in doing nothing.
+      unsigned long long int idle_period_;
+      /// Time spend in doing nothing.
+      unsigned long long int idle_all_period_;
+      /// Time spend in waiting an input/output to complete.
+      unsigned long long int iowait_period_;
+      /// Time spend in servicing hardware interrupts.
+      unsigned long long int irq_period_;
+      /// Time spend in servicing software interrupts.
+      unsigned long long int softirq_period_;
+      /// Time spend in other operating systems in a virtualized environments
+      /// Never doing this for control !
+      unsigned long long int steal_period_;
+      /// Time spend running a virtual CPU for guest operating systems
+      /// under the control of the Linux kernel
+      unsigned long long int guest_period_;
+      /// @}
+
+      double percent_;
+      void ProcessLine(std::istringstream &aCPULine);
+
+      friend class boost::serialization::access;
+
+      template<class Archive>
+	void serialize(Archive & ar, const unsigned int version)
+      {
+	ar & version;
+	ar & total_time_;
+        ar & user_mode_time_;
+        ar & nice_time_;
+        ar & system_time_;
+        ar & system_all_time_;	
+	ar & idle_time_;
+	ar & idle_all_time_;	
+	ar & iowait_time_;
+	ar & irq_time_;
+	ar & softirq_time_;
+	ar & steal_time_;
+	ar & guest_time_;
+	ar & guest_nice_time_;
+	ar & percent_;
+      }
+
+    };
+
+    /// \class This class gathers information on a computer.
+    /// This includes a list of CPU
+    class DYNAMIC_GRAPH_DLLAPI System
+    {
+    private:
+      bool init_;
+      
+    public:
+      System();
+
+      /// Read /proc/state file to extract CPU count.
+      void init();
+
+      /// Update CPU data information from /proc/stat
+      void readProcStat();
+
+      /// Friend class for serialization.
+      friend class boost::serialization::access;
+
+      /// Number of CPU.
+      unsigned int cpuNb_;
+
+      void ProcessCPULine(unsigned int cpunb,
+			  std::istringstream &aCPULine);
+    
+      /// \brief Vector of CPU informations.
+      std::vector<CPUData> vCPUData_;
+
+      /// \brief Global CPU information.
+      CPUData gCPUData_;
+
+      template<class Archive>
+	void serialize(Archive & ar, const unsigned int version)
+      {
+	ar & version;
+	ar & cpuNb_;
+	ar & gCPUData_;
+	ar & vCPUData_;	
+      }
+
+    };
+  }
+}
+
+#endif /* DYNAMIC_GRAPH_PROCESS_LIST_H_ */

src/CMakeLists.txt

@@ -30,6 +30,8 @@ ADD_LIBRARY(${LIBRARY_NAME}
   exception/exception-signal.cpp
   exception/exception-traces.cpp
 
+  mt/process-list.cpp
+
   signal/signal-array.cpp
   signal/signal-caster.cpp
   signal/signal-cast-helper.cpp

src/debug/real-time-logger.cpp

@@ -74,34 +74,66 @@ namespace dynamicgraph
   struct RealTimeLogger::thread
   {
     bool requestShutdown_;
+    int threadPolicy_;
+    int threadPriority_;
+    bool changedThreadParams;    
     boost::thread t_;
-
+    
     thread (RealTimeLogger* logger)
       : requestShutdown_ (false)
+      , threadPolicy_(SCHED_OTHER)
+      , threadPriority_(0)
+      , changedThreadParams(true)
       , t_ (&thread::spin, this, logger)
     {}
 
-    void spin (RealTimeLogger* logger)
+    void setThreadPolicy(int policy)
+    {
+      threadPolicy_ = policy;
+      changedThreadParams = true;
+      
+    }
+
+    void setPriority(int priority)
+    {
+      threadPriority_ = priority;
+      changedThreadParams = true;      
+    }
+    
+    int getThreadPolicy() { return threadPolicy_; }
+    int getThreadPriority() { return threadPriority_; }
+    
+    void changeThreadParams()
     {
-      // Change the thread's scheduler from real-time to normal and reduce its priority
       int threadPolicy;
       struct sched_param threadParam;
-      if (pthread_getschedparam (pthread_self(), &threadPolicy, &threadParam) == 0)
+      if (pthread_getschedparam (pthread_self(), &threadPolicy, &threadParam)
+	  == 0)
 	{
-	  threadPolicy = SCHED_OTHER;
-	  threadParam.sched_priority -= 5;
-	  if (threadParam.sched_priority < sched_get_priority_min (threadPolicy))
+	  threadPolicy = threadPolicy_;
+	  threadParam.sched_priority = threadPriority_;
+	  if (threadParam.sched_priority <
+	      sched_get_priority_min (threadPolicy))
 	    threadParam.sched_priority = sched_get_priority_min (threadPolicy);
 
 	  pthread_setschedparam (pthread_self(), threadPolicy, &threadParam);
+	  changedThreadParams = false;
 	}
+    }
+    
+    void spin (RealTimeLogger* logger)
+    {
+      // Change the thread's scheduler from real-time to normal
+      // and reduce its priority
 
       while (!requestShutdown_ || !logger->empty())
 	{
 	  // If the logger did not write anything, it means the buffer is empty.
 	  // Do a pause
 	  if (!logger->spinOnce())
-	    boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+	    boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+	  if (changedThreadParams)
+	    changeThreadParams();
 	}
     }
   };

src/mt/process-list.cpp

+/* Copyright LAAS, CNRS
+ * Author: O. Stasse, 2019
+ * See LICENSE file in the root directory of this repository.
+ */
+#include <fstream>
+#include <sstream>
+#include <string>
+
+#include <dynamic-graph/process-list.hh>
+
+using namespace dynamicgraph::CPU;
+CPUData::CPUData():
+  user_mode_time_(0),
+  nice_time_(0),
+  system_time_(0),
+  idle_time_(0),
+  iowait_time_(0),
+  irq_time_(0),
+  softirq_time_(0),
+  steal_time_(0),
+  guest_time_(0),
+  guest_nice_time_(0),
+  percent_(0.0)
+{
+}
+
+void CPUData::ProcessLine(std::istringstream &aCPULine)
+{
+  unsigned long long int luser_mode_time=0,
+    lnice_time=0, lsystem_time=0, lidle_time=0,
+    liowait_time=0, lirq_time=0, lsoftirq_time=0,
+    lsteal_time=0,lguest_time=0, lguest_nice_time;
+  
+  aCPULine >> luser_mode_time;
+  aCPULine >> lnice_time;
+  aCPULine >> lsystem_time;
+  aCPULine >> lidle_time;
+  aCPULine >> liowait_time;
+  aCPULine >> lirq_time;
+  aCPULine >> lsoftirq_time;
+  aCPULine >> lsteal_time;
+  aCPULine >> lguest_time;
+  aCPULine >> lguest_nice_time;
+
+  // Remove guest time already in user_time:
+  luser_mode_time -= lguest_time;
+  lnice_time -= lguest_nice_time;
+
+  // Compute cumulative time
+  unsigned long long int lidle_all_time=0,
+    lsystem_all_time=0, lguest_all_time=0, ltotal_time=0;
+
+  lidle_all_time = lidle_time + liowait_time;
+  lsystem_all_time = lsystem_time + lirq_time + lsoftirq_time;
+  lguest_all_time = lguest_time + lguest_nice_time;
+  ltotal_time = luser_mode_time + lnice_time + lsystem_all_time +
+    lidle_all_time + lsteal_time + lguest_all_time;
+
+
+  // Update periodic computation.
+  user_mode_period_  = computePeriod(luser_mode_time, user_mode_time_);
+  nice_period_       = computePeriod(lnice_time, nice_time_);
+  system_period_     = computePeriod(lsystem_time, system_time_);
+  system_all_period_ = computePeriod(lsystem_all_time,system_all_time_);
+  idle_period_       = computePeriod(lidle_time, idle_time_);
+  idle_all_period_   = computePeriod(lidle_all_time, idle_all_time_);
+  iowait_period_     = computePeriod(liowait_time, idle_time_);
+  irq_period_        = computePeriod(lirq_time, irq_time_);
+  softirq_period_    = computePeriod(lsoftirq_time, softirq_time_);
+  steal_period_      = computePeriod(lsteal_time, steal_time_);
+  guest_period_      = computePeriod(lguest_all_time, guest_time_);
+  total_period_      = computePeriod(ltotal_time, total_time_);
+
+  /// Update time.
+  user_mode_time_  = luser_mode_time;
+  nice_time_       = lnice_time;
+  system_time_     = lsystem_time;
+  system_all_time_ = lsystem_all_time;
+  idle_time_       = lidle_time;
+  idle_all_time_   = lidle_all_time;
+  iowait_time_     = liowait_time;
+  irq_time_        = lirq_time;
+  softirq_time_    = lsoftirq_time;
+  steal_time_      = lsteal_time;
+  guest_time_      = lguest_all_time;
+  total_time_      = ltotal_time;
+
+  if (total_period_!=0.0)
+    {
+      percent_ = (double)(user_mode_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( nice_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( system_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( irq_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( softirq_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( steal_period_)/(double)(total_period_) * 100.0;
+      percent_ += (double)( iowait_period_)/(double)(total_period_) * 100.0;
+    }
+  std::cout << cpu_id_ << " " << percent_ << std::endl;  
+}
+
+System::System()
+{
+  vCPUData_.clear();
+  init();
+}
+
+void System::init()
+{
+  init_ = false;
+  readProcStat();
+  init_ = true;
+}
+
+void System::ProcessCPULine(unsigned int cpunb,
+			    std::istringstream &aCPULine)
+{
+  vCPUData_[cpunb].ProcessLine(aCPULine);
+}
+
+void System::readProcStat()
+{
+  std::ifstream aif;
+  cpuNb_ = 1;
+  
+  aif.open("/proc/stat",std::ifstream::in);
+  std::string aline;
+  aline.clear();
+  while (std::getline(aif,aline))
+    {
+      // Read on line of the file
+      std::istringstream anISSLine(aline);
+      std::string line_hdr;
+      anISSLine >> line_hdr;
+
+      // Check if the line start with cpu
+      std::size_t pos = line_hdr.find("cpu");
+      std::string str_cpunbr = line_hdr.substr(pos+3);
+
+      // Check if this is the  first line
+      if (pos==0 and str_cpunbr.empty())
+	{
+	  gCPUData_.ProcessLine(anISSLine);
+	  gCPUData_.cpu_id_=-1;
+	}
+      else
+	{
+	  // If not then check if there is a CPU number
+	  if (pos==0)
+	    {
+	      std::istringstream iss(str_cpunbr);
+	      unsigned int lcpunb;
+	      iss >> lcpunb;
+	      // If we did not initialize
+	      if (!init_)
+		{
+		  // Count the number of CPU.
+		  if (lcpunb>cpuNb_)
+		    cpuNb_ = lcpunb;
+		}
+	      else
+		// Otherwise process the line.
+		ProcessCPULine(lcpunb,anISSLine);
+	    }
+	}
+    }
+
+  if (!init_)
+    {
+      /// The number of CPU has been detected by going through /proc/stat.
+      vCPUData_.resize(cpuNb_+1);
+      for(int i=0;i<(int)cpuNb_;i++)
+	vCPUData_[i].cpu_id_ = i;
+    }
+  aif.close();
+}

tests/CMakeLists.txt

@@ -62,3 +62,5 @@ TARGET_LINK_LIBRARIES(debug-tracer tracer)
 DYNAMIC_GRAPH_TEST(debug-logger)
 DYNAMIC_GRAPH_TEST(debug-logger-winit)
 DYNAMIC_GRAPH_TEST(command-test)
+DYNAMIC_GRAPH_TEST(test-mt)
+TARGET_LINK_LIBRARIES(test-mt tracer)

tests/real-time-logger.cpp

@@ -43,6 +43,20 @@ BOOST_AUTO_TEST_CASE (monothread)
 
 BOOST_AUTO_TEST_CASE (multithread)
 {
+  // The part of the code changing priority will only be effective
+  // if this test is run as root. Otherwise it behaves like a classical thread.
+  // Test confirms that in this case, it runs with a priority -51
+  // and that the thread for logging is running on SCHED_OTHER
+  // with a nice priority (20).
+  int threadPolicy;
+  struct sched_param threadParam;
+  if (pthread_getschedparam (pthread_self(), &threadPolicy, &threadParam)==0)
+    {
+      threadPolicy = SCHED_RR;
+      threadParam.sched_priority = 50;
+      pthread_setschedparam (pthread_self(), threadPolicy, &threadParam);
+    }
+
   RealTimeLogger& rtl = RealTimeLogger::instance();
   dgADD_OSTREAM_TO_RTLOG (std::cout);