From fab8909c52ba2f78bbd3ab676ba1c9343d5be1fd Mon Sep 17 00:00:00 2001
From: fbailly <fbailly@laas.fr>
Date: Mon, 28 Jan 2019 19:52:12 +0100
Subject: [PATCH] [topic/logger_sigHelper] added logger.h and modified
signal-helper.h from sot-torque-control
---
include/dynamic-graph/CMakeLists.txt | 4 +-
include/dynamic-graph/logger.h | 216 ++++++++++++++++++++++++++
include/dynamic-graph/signal-helper.h | 38 ++++-
src/CMakeLists.txt | 1 +
src/debug/logger.cpp | 108 +++++++++++++
5 files changed, 365 insertions(+), 2 deletions(-)
create mode 100644 include/dynamic-graph/logger.h
create mode 100644 src/debug/logger.cpp
diff --git a/include/dynamic-graph/CMakeLists.txt b/include/dynamic-graph/CMakeLists.txt
index a8a7ae5..efec6a1 100644
--- a/include/dynamic-graph/CMakeLists.txt
+++ b/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}
diff --git a/include/dynamic-graph/logger.h b/include/dynamic-graph/logger.h
new file mode 100644
index 0000000..f40a0ec
--- /dev/null
+++ b/include/dynamic-graph/logger.h
@@ -0,0 +1,216 @@
+/*
+ * 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__
diff --git a/include/dynamic-graph/signal-helper.h b/include/dynamic-graph/signal-helper.h
index 87bbec3..0282e34 100644
--- a/include/dynamic-graph/signal-helper.h
+++ b/include/dynamic-graph/signal-helper.h
@@ -1,5 +1,7 @@
/*
- * 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__
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3e16737..738a664 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/debug/logger.cpp b/src/debug/logger.cpp
new file mode 100644
index 0000000..412de96
--- /dev/null
+++ b/src/debug/logger.cpp
@@ -0,0 +1,108 @@
+/*
+ * 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
+
--
GitLab