Added plugin "Reader" to read files generated by "Tracer".

include/CMakeLists.txt

@@ -22,7 +22,6 @@ exception-feature.h
 exception-signal.h
 exception-task.h
 exception-tools.h
-exception-traces.h
 
 #signal
 signal-cast.h
@@ -88,6 +87,9 @@ sot.h
 rotation-simple.h
 weighted-sot.h
 solver-hierarchical-inequalities.h
+
+#reader
+reader.h
 )
 
 # Recreate correct path for the headers

include/sot-core/exception-traces.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      exception-traces.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-#ifndef __SOT_TRACES_EXCEPTION_H
-#define __SOT_TRACES_EXCEPTION_H
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-#include <sot-core/exception-abstract.h>
-#include <sot-core/sot-core-api.h>
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-namespace sot {
-
-/* \class ExceptionTraces
- */
-class SOT_CORE_EXPORT ExceptionTraces 
-:public ExceptionAbstract
-
-{
- public:
-  enum ErrorCodeEnum
-    {
-      GENERIC = ExceptionAbstract::TRACES
-
-      ,NOT_OPEN
-    };
-
-  static const std::string EXCEPTION_NAME;
-  virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
-
-public:
-
-  ExceptionTraces ( const ExceptionTraces::ErrorCodeEnum& errcode,
-		       const std::string & msg = "" );
-  ExceptionTraces( const ExceptionTraces::ErrorCodeEnum& errcode,
-			const std::string & msg,const char* format, ... );
-  virtual ~ExceptionTraces( void ){}
-
-
-};
-
-} // namespace sot
-
-
-
-#endif /* #ifndef __SOT_TRACES_EXCEPTION_H */
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */

include/sot-core/reader.h

+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File:      sotReader.h
+ * Project:   SOT
+ * Author:    Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ *  $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_TRACER_H__
+#define __SOT_TRACER_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* STD */
+#include <string>
+#include <vector>
+#include <list>
+#include <boost/function.hpp>
+#include <fstream>
+
+/* SOT & DG*/
+#include <dynamic-graph/signal-base.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/exception-traces.h>
+#include <sot-core/flags.h>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32) 
+#  if defined (sotReader_EXPORTS)
+#    define SOTREADER_EXPORT __declspec(dllexport)
+#  else  
+#    define SOTREADER_EXPORT __declspec(dllimport)
+#  endif 
+#else
+#  define SOTREADER_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- TRACER ---------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dg = dynamicgraph;
+
+class SOTREADER_EXPORT sotReader
+: public dg::Entity
+{
+ public: 
+  static const std::string CLASS_NAME;
+  virtual const std::string& getClassName( void ) { return CLASS_NAME; }
+
+ public: 
+
+  dg::SignalPtr< sot::Flags,int > selectionSIN;
+  dg::SignalTimeDependent<ml::Vector,int> vectorSOUT;
+  dg::SignalTimeDependent<ml::Matrix,int> matrixSOUT;
+
+ public:
+  sotReader( const std::string n );
+  virtual ~sotReader( void ){}
+
+  void load( const std::string& filename );
+  void clear( void );
+  void rewind( void );
+
+ protected:
+
+  typedef std::list< std::vector<double> > DataType;
+  DataType dataSet;
+  DataType::const_iterator currentData;
+  bool iteratorSet;
+
+  unsigned int nbRows,nbCols;
+
+  ml::Vector& getNextData( ml::Vector& res, const unsigned int time );
+  ml::Matrix& getNextMatrix( ml::Matrix& res, const unsigned int time );
+
+ public:
+  /* --- PARAMS --- */
+  void display( std::ostream& os ) const;
+  virtual void commandLine( const std::string& cmdLine
+			    ,std::istringstream& cmdArgs
+			    ,std::ostream& os );
+};
+
+
+
+
+
+#endif /* #ifndef __SOT_TRACER_H__ */
+
+

src/CMakeLists.txt

@@ -41,6 +41,8 @@ SET(plugins
 	feature/feature-visual-point
 	feature/feature-task
 	feature/feature-line-distance
+	
+	traces/reader
 )
 
 SET(LIBRARY_NAME ${SOTCORE_LIB_NAME})

src/traces/reader.cpp

+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File:      sotReader.cpp
+ * Project:   SOT
+ * Author:    Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ *  $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-core/reader.h>
+#include <sot-core/debug.h>
+
+#include <boost/bind.hpp>
+#include <sstream>
+#include <dynamic-graph/factory.h>
+
+using namespace dynamicgraph;
+using namespace sot;
+using namespace std;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader,"Reader");
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+sotReader::sotReader( const std::string n )
+  :Entity(n)
+  ,selectionSIN( NULL,"Reader("+n+")::input(flag)::selec" )
+   ,vectorSOUT( boost::bind(&sotReader::getNextData,this,_1,_2),
+	      sotNOSIGNAL,
+	      "Reader("+n+")::vector" )
+   ,matrixSOUT( boost::bind(&sotReader::getNextMatrix,this,_1,_2),
+		vectorSOUT,
+		"Reader("+n+")::matrix" )
+   ,dataSet()
+   ,currentData()
+   ,iteratorSet(false)
+  ,nbRows(0),nbCols(0)
+{
+  signalRegistration( selectionSIN<<vectorSOUT<<matrixSOUT );
+  selectionSIN = true;
+  vectorSOUT.setNeedUpdateFromAllChildren(true);
+}  
+
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+void sotReader::
+load( const string& filename )
+{
+  sotDEBUGIN(15);
+
+  std::ifstream datafile( filename.c_str() );
+  const unsigned int SIZE=1024;
+  char buffer[SIZE]; 
+  std::vector<double> newline;
+  while( datafile.good() )
+    {
+      datafile.getline( buffer,SIZE );
+      const unsigned int gcount = datafile.gcount(); 
+      if( gcount>=SIZE ) { /* TODO read error, line to long. */ }
+      std::istringstream iss(buffer);
+      newline.clear();
+      sotDEBUG(25) << "Get line = '" << buffer << "'" << std::endl;
+      while( 1 ) 
+	{
+	  double x; iss>>x;
+	  if(! iss.fail() )newline.push_back(x); else break;
+	  sotDEBUG(45) << "New data = " << x  << std::endl;
+	}
+      if( newline.size()>0 )dataSet.push_back( newline );
+    }
+
+  sotDEBUGOUT(15);
+}
+
+void sotReader::clear( void )
+{
+  sotDEBUGIN(15);
+
+  dataSet.clear();
+  iteratorSet = false;
+
+  sotDEBUGOUT(15);
+}
+
+void sotReader::
+rewind( void )
+{
+  sotDEBUGIN(15);
+  iteratorSet = false;
+  sotDEBUGOUT(15);
+}
+
+ml::Vector& sotReader::
+getNextData( ml::Vector& res, const unsigned int time )
+{
+  sotDEBUGIN(15);
+
+  if(! iteratorSet ) 
+    {
+      sotDEBUG(15) << "Start the list" << std::endl; 
+      currentData = dataSet.begin(); iteratorSet=true;
+    }
+  else if( currentData!=dataSet.end() ){ ++currentData;  }
+
+  if( currentData==dataSet.end() )
+    {
+      sotDEBUGOUT(15);
+      return res;
+    }
+   
+  const Flags& selection = selectionSIN(time);
+  const std::vector<double> & curr =  *currentData;
+
+  unsigned int dim=0;
+  for( unsigned int i=0;i<curr.size();++i ) if( selection(i) ) dim++;
+
+  res.resize(dim);
+  unsigned int cursor=0;
+  for( unsigned int i=0;i<curr.size();++i ) 
+    if( selection(i) ) res(cursor++)=curr[i];
+  
+  sotDEBUGOUT(15);
+  return res;
+}
+
+ml::Matrix& sotReader::
+getNextMatrix( ml::Matrix& res, const unsigned int time )
+{
+  sotDEBUGIN(15);
+  const ml::Vector& vect = vectorSOUT(time);
+  if( vect.size()<nbRows*nbCols ) return res;
+
+  res.resize( nbRows,nbCols );
+  for( unsigned int i=0;i<nbRows;++i )
+    for( unsigned int j=0;j<nbCols;++j )
+      res(i,j)=vect(i*nbCols+j);
+
+  sotDEBUGOUT(15);
+  return res;
+}
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+void sotReader::
+display( std::ostream& os ) const
+{
+  os << CLASS_NAME << " " << name << endl;
+}
+
+
+std::ostream& operator<< ( std::ostream& os,const sotReader& t )
+{
+  t.display(os);
+  return os;
+}
+
+void sotReader::
+commandLine( const std::string& cmdLine
+	     ,std::istringstream& cmdArgs
+	     ,std::ostream& os )
+{
+  if( cmdLine=="help" )
+    {
+      os << "Reader: "<<endl
+	 << "  - load" << endl;
+	
+      Entity::commandLine( cmdLine,cmdArgs,os );
+    }
+  else if( cmdLine=="load" )
+    {
+      string filename;
+      cmdArgs>>ws>>filename;
+      load(filename);
+    }
+  else if( cmdLine=="rewind" )
+    {
+      cmdArgs>>ws;
+      rewind( );
+    }
+  else if( cmdLine=="clear" )
+    {
+      cmdArgs>>ws;
+      clear( );
+    }
+  else if( cmdLine=="resize" )
+    {
+      cmdArgs >>ws;
+      if( cmdArgs.good() ) { cmdArgs >> nbRows >>nbCols; }
+      else {  os << "Matrix size = " << nbRows << " x " <<nbCols <<std::endl; }
+    }
+  else { Entity::commandLine( cmdLine,cmdArgs,os ); }
+
+
+
+}