[topic/logger_sigHelper] added logger.h and modified signal-helper.h from sot-torque-control

include/dynamic-graph/CMakeLists.txt

@@ -47,7 +47,9 @@ SET(${PROJECT_NAME}_HEADERS
   command-direct-setter.h
   command-bind.h
   all-commands.h
-  )
+
+  logger.h
+    )
 
 INSTALL(FILES ${${PROJECT_NAME}_HEADERS}
   DESTINATION include/${PROJECT_NAME}

include/dynamic-graph/logger.h

+/*
+ * Copyright 2015, 2019
+ * LAAS-CNRS
+ * Andrea Del Prete, François Bailly,
+ *
+ * This file is part of dynamic-graph.
+ * See license file.
+ */
+
+#ifndef __dynamic_graph_logger_H__
+#define __dynamic_graph_logger_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (logger_EXPORTS)
+#    define LOGGER_EXPORT __declspec(dllexport)
+#  else
+#    define LOGGER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define LOGGER_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/signal-helper.h>
+#include <map>
+#include <iomanip> // std::setprecision
+#include "boost/assign.hpp"
+
+
+namespace dynamicgraph {
+
+//#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
+#define LOGGER_VERBOSITY_ALL
+
+#define SEND_MSG(msg,type)         sendMsg(msg,type,__FILE__,__LINE__)
+
+#ifdef LOGGER_VERBOSITY_ERROR
+  #define SEND_DEBUG_STREAM_MSG(msg)
+  #define SEND_INFO_STREAM_MSG(msg)
+  #define SEND_WARNING_STREAM_MSG(msg)
+  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_WARNING_ERROR
+  #define SEND_DEBUG_STREAM_MSG(msg)
+  #define SEND_INFO_STREAM_MSG(msg)\
+  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
+  #define SEND_DEBUG_STREAM_MSG(msg)
+  #define SEND_INFO_STREAM_MSG(msg)     SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_ALL
+  #define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG_STREAM)
+  #define SEND_INFO_STREAM_MSG(msg)   SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+  /** Enum representing the different kind of messages.
+   */
+  enum MsgType
+  {
+    MSG_TYPE_DEBUG          =0,
+    MSG_TYPE_INFO           =1,
+    MSG_TYPE_WARNING        =2,
+    MSG_TYPE_ERROR          =3,
+    MSG_TYPE_DEBUG_STREAM   =4,
+    MSG_TYPE_INFO_STREAM    =5,
+    MSG_TYPE_WARNING_STREAM =6,
+    MSG_TYPE_ERROR_STREAM   =7
+  };
+
+  template<typename T>
+  std::string toString(const T& v, const int precision=3, const int width=-1)
+  {
+    std::stringstream ss;
+    if(width>precision)
+      ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v;
+    else
+      ss<<std::fixed<<std::setprecision(precision)<<v;
+    return ss.str();
+  }
+
+  template<typename T>
+  std::string toString(const std::vector<T>& v, const int precision=3, const int width=-1,
+                       const std::string separator=", ")
+  {
+    std::stringstream ss;
+    if(width>precision)
+    {
+      for(int i=0; i<v.size()-1; i++)
+        ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
+      ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
+    }
+    else
+    {
+      for(int i=0; i<v.size()-1; i++)
+        ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
+      ss<<std::fixed<<std::setprecision(precision)<<v[v.size()-1];
+    }
+
+    return ss.str();
+  }
+
+//      template<typename T, int N>
+//      std::string toString(const Eigen::Matrix<T, N, 1, 0, N, 1>& v, const std::string separator=", ",
+//                           const int precision=3, const int width=-1)
+  template<typename T>
+  std::string toString(const Eigen::MatrixBase<T>& v,
+                       const int precision=3, const int width=-1, const std::string separator=", ")
+  {
+    std::stringstream ss;
+    if(width>precision)
+    {
+      for(int i=0; i<v.size()-1; i++)
+        ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
+      ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
+    }
+    else
+    {
+      for(int i=0; i<v.size()-1; i++)
+        ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
+      ss<<std::setprecision(precision)<<v[v.size()-1];
+    }
+
+    return ss.str();
+  }
+
+  enum LoggerVerbosity
+  {
+    VERBOSITY_ALL,
+    VERBOSITY_INFO_WARNING_ERROR,
+    VERBOSITY_WARNING_ERROR,
+    VERBOSITY_ERROR,
+    VERBOSITY_NONE
+  };
+
+  /** A simple class for logging messages
+  */
+  class Logger
+  {
+  public:
+
+    /** Constructor */
+    Logger(double timeSample=0.001, double streamPrintPeriod=1.0);
+
+    /** Destructor */
+    ~Logger(){}
+
+    /** Method to be called at every control iteration
+       * to decrement the internal Logger's counter. */
+    void countdown();
+
+    /** Print the specified message on standard output if the verbosity level
+     * allows it. The file name and the line number are used to identify
+     * the point where sendMsg is called so that streaming messages are
+     * printed only every streamPrintPeriod iterations.
+     */
+    void sendMsg(std::string msg, MsgType type, const char* file="", int line=0);
+
+    /** Set the sampling time at which the method countdown()
+       * is going to be called. */
+    bool setTimeSample(double t);
+
+    /** Set the time period for printing of streaming messages. */
+    bool setStreamPrintPeriod(double s);
+
+    /** Set the verbosity level of the logger. */
+    void setVerbosity(LoggerVerbosity lv);
+
+  protected:
+    LoggerVerbosity m_lv;                /// verbosity of the logger
+    double          m_timeSample;        /// specify the period of call of the countdown method
+    double          m_streamPrintPeriod; /// specify the time period of the stream prints
+    double          m_printCountdown;    /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
+
+    /** Pointer to the dynamic structure which holds the collection of streaming messages */
+    std::map<std::string, double> m_stream_msg_counters;
+
+    bool isStreamMsg(MsgType m)
+    { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_WARNING_STREAM; }
+
+    bool isDebugMsg(MsgType m)
+    { return m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_DEBUG; }
+
+    bool isInfoMsg(MsgType m)
+    { return m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_INFO; }
+
+    bool isWarningMsg(MsgType m)
+    { return m==MSG_TYPE_WARNING_STREAM || m==MSG_TYPE_WARNING; }
+
+    bool isErrorMsg(MsgType m)
+    { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_ERROR; }
+  };
+
+  /** Method to get the logger (singleton). */
+  Logger& getLogger();
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_torque_control_logger_H__

include/dynamic-graph/signal-helper.h

 /*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ * Copyright 2011, 2019 
+ * LAAS-CNRS
+ * Nicolas Mansard, François Bailly 
  *
  * This file is part of dynamic-graph.
  * dynamic-graph is free software: you can redistribute it and/or
@@ -23,26 +25,60 @@
 #include <dynamic-graph/entity.h>
 #include <dynamic-graph/signal-ptr.h>
 #include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/linear-algebra.h> 
 
 /* --- MACROS ---------------------------------------------------------- */
+#define SIGNAL_OUT_FUNCTION_NAME(name) name##SOUT_function
 
 #define DECLARE_SIGNAL( name,IO,type )    ::dynamicgraph::Signal<type,int> name##S##IO
 #define CONSTRUCT_SIGNAL( name,IO,type )  name##S##IO( getClassName()+"("+getName()+")::"+#IO+"put("+#type+")::"+#name )
+#define BIND_SIGNAL_TO_FUNCTION(name,IO,type) m_##name##S##IO.setFunction(boost::bind(&EntityClassName::SIGNAL_OUT_FUNCTION_NAME(name),this,_1,_2));
+
+/**/
 
 #define DECLARE_SIGNAL_IN( name,type )    ::dynamicgraph::SignalPtr<type,int> name##SIN
 #define CONSTRUCT_SIGNAL_IN( name,type )  name##SIN( NULL,getClassName()+"("+getName()+")::input("+#type+")::"+#name )
 
+/**/
+
+#define DECLARE_SIGNAL_OUT_FUNCTION(name,type) \
+  type& SIGNAL_OUT_FUNCTION_NAME(name)(type&,int)
+
+#define DEFINE_SIGNAL_OUT_FUNCTION(name,type) \
+  type& EntityClassName::SIGNAL_OUT_FUNCTION_NAME(name)(type& s,int iter)
+
 #define SIGNAL_OUT_FUNCTION( name )  name##SOUT_function
+
 #define DECLARE_SIGNAL_OUT( name,type )                         \
   public:                                                       \
     ::dynamicgraph::SignalTimeDependent<type,int> name##SOUT;	\
   protected:                                                    \
   type& SIGNAL_OUT_FUNCTION(name)( type&,int )
+
 #define CONSTRUCT_SIGNAL_OUT( name,type,dep )		\
   name##SOUT( boost::bind(&  EntityClassName::name##SOUT_function,this,_1,_2), \
 	      dep,getClassName()+"("+getName()+")::output("+#type+")::"+#name )
 
 
 
+/**************** INNER SIGNALS *******************/
+#define SIGNAL_INNER_FUNCTION_NAME(name) name##SINNER_function
+
+#define DECLARE_SIGNAL_INNER_FUNCTION(name,type) \
+  type& SIGNAL_INNER_FUNCTION_NAME(name)(type&,int)
+
+#define DEFINE_SIGNAL_INNER_FUNCTION(name,type) \
+  type& EntityClassName::SIGNAL_INNER_FUNCTION_NAME(name)(type& s,int iter)
+
+#define DECLARE_SIGNAL_INNER(name,type)                           \
+  public:                                                       \
+    ::dynamicgraph::SignalTimeDependent<type,int> m_##name##SINNER;\
+  protected:                                                    \
+    DECLARE_SIGNAL_INNER_FUNCTION(name,type)
+
+#define CONSTRUCT_SIGNAL_INNER( name,type,dep )		        \
+  m_##name##SINNER( boost::bind(&EntityClassName::name##SINNER_function,this,_1,_2), \
+              dep,getClassName()+"("+getName()+")::inner("+#type+")::"+#name )
+
 
 #endif // __dynamic_graph_signal_helper_H__

src/CMakeLists.txt

@@ -41,6 +41,7 @@ ADD_LIBRARY(${LIBRARY_NAME}
   SHARED
   debug/debug.cpp
   debug/real-time-logger.cpp
+  debug/logger.cpp
 
   dgraph/entity.cpp
   dgraph/factory.cpp

src/debug/logger.cpp

+/*
+ * Copyright 2015, 2019 
+ * LAAS-CNRS
+ * Andrea Del Prete, François Bailly 
+ * 
+ * This file is part of dynamic-graph.
+ * See license file.
+ */
+
+#ifndef WIN32
+  #include <sys/time.h>
+#else
+  #include <Windows.h>
+#endif
+
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>      // std::setprecision
+#include <dynamic-graph/logger.h>
+
+namespace dynamicgraph
+{
+
+    using namespace std;
+
+    Logger& getLogger()
+    {
+      static Logger l(0.001, 1.0);
+      return l;
+    }
+
+    Logger::Logger(double timeSample, double streamPrintPeriod)
+      : m_timeSample(timeSample),
+        m_streamPrintPeriod(streamPrintPeriod),
+        m_printCountdown(0.0)
+    {
+#ifdef LOGGER_VERBOSITY_ERROR
+        m_lv = VERBOSITY_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_WARNING_ERROR
+        m_lv = VERBOSITY_WARNING_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
+        m_lv = VERBOSITY_INFO_WARNING_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_ALL
+        m_lv = VERBOSITY_ALL;
+#endif
+    }
+
+    void Logger::countdown()
+    {
+      if(m_printCountdown<0.0)
+        m_printCountdown = m_streamPrintPeriod;
+      m_printCountdown -= m_timeSample;
+    }
+
+    void Logger::sendMsg(string msg, MsgType type, const char* file, int line)
+    {
+      if(m_lv==VERBOSITY_NONE ||
+        (m_lv==VERBOSITY_ERROR && !isErrorMsg(type)) ||
+        (m_lv==VERBOSITY_WARNING_ERROR && !(isWarningMsg(type) || isErrorMsg(type))) ||
+        (m_lv==VERBOSITY_INFO_WARNING_ERROR && isDebugMsg(type)))
+        return;
+
+      // if print is allowed by current verbosity level
+      if(isStreamMsg(type))
+      {
+        // check whether counter already exists
+        string id = file+toString(line);
+        map<string,double>::iterator it = m_stream_msg_counters.find(id);
+        if(it == m_stream_msg_counters.end())
+        {
+          // if counter doesn't exist then add one
+          m_stream_msg_counters.insert(make_pair(id, 0.0));
+          it = m_stream_msg_counters.find(id);
+        }
+
+        // if counter is greater than 0 then decrement it and do not print
+        if(it->second>0.0)
+        {
+          it->second -= m_timeSample;
+          return;
+        }
+        else  // otherwise reset counter and print
+          it->second = m_streamPrintPeriod;
+      }
+      printf("%s\n", msg.c_str());
+      fflush(stdout); // Prints to screen or whatever your standard out is
+    }
+
+    bool Logger::setTimeSample(double t)
+    {
+      if(t<=0.0)
+        return false;
+      m_timeSample = t;
+      return true;
+    }
+
+    bool Logger::setStreamPrintPeriod(double s)
+    {
+      if(s<=0.0)
+        return false;
+      m_streamPrintPeriod = s;
+      return true;
+    }
+} // namespace dynamicgraph
+