Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/trace-player.cpp	Lines:	0	83	0.0 %
Date:	2023-06-05 17:45:50	Branches:	0	234	0.0 %


/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 */

#include <dynamic-graph/factory.h>

#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/trace-player.hh>
#include <tsid/utils/statistics.hpp>
#include <tsid/utils/stop-watch.hpp>

namespace dynamicgraph {
namespace sot {
namespace torque_control {
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace dynamicgraph::sot::torque_control;

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef TracePlayer EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TracePlayer, "TracePlayer");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
TracePlayer::TracePlayer(const std::string& name)
    : Entity(name), CONSTRUCT_SIGNAL_OUT(trigger, int, sotNOSIGNAL) {
  Entity::signalRegistration(m_triggerSOUT);

  /* Commands. */
  addCommand("addOutputSignal",
             makeCommandVoid2(
                 *this, &TracePlayer::addOutputSignal,
                 docCommandVoid2(
                     "Add a new output signal",
                     "Name of the text file where to read the data (string)",
                     "Name of the output signal (string)")));

  addCommand(
      "playNext",
      makeCommandVoid0(*this, &TracePlayer::playNext,
                       docCommandVoid0("Update all the output signals.")));

  addCommand("rewind",
             makeCommandVoid0(*this, &TracePlayer::rewind,
                              docCommandVoid0("Rewind all the data.")));

  addCommand("clear", makeCommandVoid0(
                          *this, &TracePlayer::clear,
                          docCommandVoid0("Clear all the output signals.")));
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

DEFINE_SIGNAL_OUT_FUNCTION(trigger, int) {
  std::string astr = toString(iter);
  playNext();
  return s;
}

/* --- COMMANDS ---------------------------------------------------------- */

void TracePlayer::addOutputSignal(const string& fileName,
                                  const string& signalName) {
  // check there is no other signal with the same name
  if (m_outputSignals.find(signalName) != m_outputSignals.end())
    return SEND_MSG("It already exists a signal with name " + signalName,
                    MSG_TYPE_ERROR);

  // read the text file
  std::ifstream datafile(fileName.c_str());
  if (datafile.fail())
    return SEND_MSG("Error trying to read the file " + fileName,
                    MSG_TYPE_ERROR);

  const unsigned int SIZE = 1024;
  char buffer[SIZE];
  std::vector<double> newline;
  int nbLines = 0;
  bool firstIter = true;
  std::size_t size = -1;
  string fileNameShort = fileName.substr(1 + fileName.find_last_of("/"));
  while (datafile.good()) {
    datafile.getline(buffer, SIZE);
    const std::size_t gcount = datafile.gcount();
    if (gcount >= SIZE)
      return SEND_MSG("Read error: line " + toString(nbLines) +
                          " too long in file " + fileNameShort,
                      MSG_TYPE_ERROR);

    std::istringstream iss(buffer);
    newline.clear();
    double x;
    iss >> x;  // discard the first value, which is the time step
    while (1) {
      iss >> x;
      if (iss.fail()) break;
      newline.push_back(x);
    }

    if (newline.size() > 0) {
      if (firstIter)
        size = newline.size();
      else if (size != newline.size()) {
        SEND_MSG("In file " + fileNameShort +
                     " nb of elements in each line changed from " +
                     toString(size) + " to " + toString(newline.size()) +
                     " at line " + toString(nbLines),
                 MSG_TYPE_WARNING);
        size = newline.size();
      }
      m_data[signalName].push_back(
          Eigen::Map<Vector>(&newline[0], newline.size()));
      nbLines++;
    }
  }
  SEND_MSG("Finished reading " + toString(nbLines) + " lines of " +
               toString(size) + " elements from file " + fileNameShort,
           MSG_TYPE_INFO);
  m_dataPointers[signalName] = m_data[signalName].begin();

  // create a new output signal
  m_outputSignals[signalName] =
      new OutputSignalType(getClassName() + "(" + getName() +
                           ")::output(dynamicgraph::Vector)::" + signalName);

  // register the new signal
  m_triggerSOUT.addDependency(*m_outputSignals[signalName]);
  Entity::signalRegistration(*m_outputSignals[signalName]);
}

void TracePlayer::playNext() {
  typedef std::map<std::string, OutputSignalType*>::iterator it_type;
  for (it_type it = m_outputSignals.begin(); it != m_outputSignals.end();
       it++) {
    const string& signalName = it->first;
    OutputSignalType* signal = it->second;
    DataPointerType& dataPointer = m_dataPointers[signalName];
    const DataHistoryType& dataSet = m_data[signalName];

    if (dataPointer != dataSet.end()) ++dataPointer;

    if (dataPointer == dataSet.end())
      SEND_WARNING_STREAM_MSG("Reached end of dataset for signal " +
                              signalName);
    else
      signal->setConstant(*dataPointer);
  }
}

void TracePlayer::rewind() {
  typedef std::map<std::string, DataPointerType>::iterator it_type;
  for (it_type it = m_dataPointers.begin(); it != m_dataPointers.end(); it++) {
    const string& signalName = it->first;
    DataPointerType& dataPointer = it->second;
    const DataHistoryType& dataSet = m_data[signalName];
    dataPointer = dataSet.begin();
  }
}

void TracePlayer::clear() {
  m_data.clear();
  m_dataPointers.clear();
  m_outputSignals.clear();
}

/* --- PROTECTED MEMBER METHODS
 * ---------------------------------------------------------- */

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void TracePlayer::display(std::ostream& os) const {
  os << "TracePlayer " << getName();
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2017, Andrea Del Prete, LAAS-CNRS
3			*
4			*/
5
6			#include <dynamic-graph/factory.h>
7
8			#include <sot/core/debug.hh>
9			#include <sot/torque_control/commands-helper.hh>
10			#include <sot/torque_control/trace-player.hh>
11			#include <tsid/utils/statistics.hpp>
12			#include <tsid/utils/stop-watch.hpp>
13
14			namespace dynamicgraph {
15			namespace sot {
16			namespace torque_control {
17			namespace dynamicgraph = ::dynamicgraph;
18			using namespace dynamicgraph;
19			using namespace dynamicgraph::command;
20			using namespace std;
21			using namespace dynamicgraph::sot::torque_control;
22
23			/// Define EntityClassName here rather than in the header file
24			/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
25			typedef TracePlayer EntityClassName;
26
27			/* --- DG FACTORY ---------------------------------------------------- */
28			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TracePlayer, "TracePlayer");
29
30			/* ------------------------------------------------------------------- */
31			/* --- CONSTRUCTION -------------------------------------------------- */
32			/* ------------------------------------------------------------------- */
33			TracePlayer::TracePlayer(const std::string& name)
34			: Entity(name), CONSTRUCT_SIGNAL_OUT(trigger, int, sotNOSIGNAL) {
35			Entity::signalRegistration(m_triggerSOUT);
36
37			/* Commands. */
38			addCommand("addOutputSignal",
39			makeCommandVoid2(
40			*this, &TracePlayer::addOutputSignal,
41			docCommandVoid2(
42			"Add a new output signal",
43			"Name of the text file where to read the data (string)",
44			"Name of the output signal (string)")));
45
46			addCommand(
47			"playNext",
48			makeCommandVoid0(*this, &TracePlayer::playNext,
49			docCommandVoid0("Update all the output signals.")));
50
51			addCommand("rewind",
52			makeCommandVoid0(*this, &TracePlayer::rewind,
53			docCommandVoid0("Rewind all the data.")));
54
55			addCommand("clear", makeCommandVoid0(
56			*this, &TracePlayer::clear,
57			docCommandVoid0("Clear all the output signals.")));
58			}
59
60			/* ------------------------------------------------------------------- */
61			/* --- SIGNALS ------------------------------------------------------- */
62			/* ------------------------------------------------------------------- */
63
64			DEFINE_SIGNAL_OUT_FUNCTION(trigger, int) {
65			std::string astr = toString(iter);
66			playNext();
67			return s;
68			}
69
70			/* --- COMMANDS ---------------------------------------------------------- */
71
72			void TracePlayer::addOutputSignal(const string& fileName,
73			const string& signalName) {
74			// check there is no other signal with the same name
75			if (m_outputSignals.find(signalName) != m_outputSignals.end())
76			return SEND_MSG("It already exists a signal with name " + signalName,
77			MSG_TYPE_ERROR);
78
79			// read the text file
80			std::ifstream datafile(fileName.c_str());
81			if (datafile.fail())
82			return SEND_MSG("Error trying to read the file " + fileName,
83			MSG_TYPE_ERROR);
84
85			const unsigned int SIZE = 1024;
86			char buffer[SIZE];
87			std::vector<double> newline;
88			int nbLines = 0;
89			bool firstIter = true;
90			std::size_t size = -1;
91			string fileNameShort = fileName.substr(1 + fileName.find_last_of("/"));
92			while (datafile.good()) {
93			datafile.getline(buffer, SIZE);
94			const std::size_t gcount = datafile.gcount();
95			if (gcount >= SIZE)
96			return SEND_MSG("Read error: line " + toString(nbLines) +
97			" too long in file " + fileNameShort,
98			MSG_TYPE_ERROR);
99
100			std::istringstream iss(buffer);
101			newline.clear();
102			double x;
103			iss >> x; // discard the first value, which is the time step
104			while (1) {
105			iss >> x;
106			if (iss.fail()) break;
107			newline.push_back(x);
108			}
109
110			if (newline.size() > 0) {
111			if (firstIter)
112			size = newline.size();
113			else if (size != newline.size()) {
114			SEND_MSG("In file " + fileNameShort +
115			" nb of elements in each line changed from " +
116			toString(size) + " to " + toString(newline.size()) +
117			" at line " + toString(nbLines),
118			MSG_TYPE_WARNING);
119			size = newline.size();
120			}
121			m_data[signalName].push_back(
122			Eigen::Map<Vector>(&newline[0], newline.size()));
123			nbLines++;
124			}
125			}
126			SEND_MSG("Finished reading " + toString(nbLines) + " lines of " +
127			toString(size) + " elements from file " + fileNameShort,
128			MSG_TYPE_INFO);
129			m_dataPointers[signalName] = m_data[signalName].begin();
130
131			// create a new output signal
132			m_outputSignals[signalName] =
133			new OutputSignalType(getClassName() + "(" + getName() +
134			")::output(dynamicgraph::Vector)::" + signalName);
135
136			// register the new signal
137			m_triggerSOUT.addDependency(*m_outputSignals[signalName]);
138			Entity::signalRegistration(*m_outputSignals[signalName]);
139			}
140
141			void TracePlayer::playNext() {
142			typedef std::map<std::string, OutputSignalType*>::iterator it_type;
143			for (it_type it = m_outputSignals.begin(); it != m_outputSignals.end();
144			it++) {
145			const string& signalName = it->first;
146			OutputSignalType* signal = it->second;
147			DataPointerType& dataPointer = m_dataPointers[signalName];
148			const DataHistoryType& dataSet = m_data[signalName];
149
150			if (dataPointer != dataSet.end()) ++dataPointer;
151
152			if (dataPointer == dataSet.end())
153			SEND_WARNING_STREAM_MSG("Reached end of dataset for signal " +
154			signalName);
155			else
156			signal->setConstant(*dataPointer);
157			}
158			}
159
160			void TracePlayer::rewind() {
161			typedef std::map<std::string, DataPointerType>::iterator it_type;
162			for (it_type it = m_dataPointers.begin(); it != m_dataPointers.end(); it++) {
163			const string& signalName = it->first;
164			DataPointerType& dataPointer = it->second;
165			const DataHistoryType& dataSet = m_data[signalName];
166			dataPointer = dataSet.begin();
167			}
168			}
169
170			void TracePlayer::clear() {
171			m_data.clear();
172			m_dataPointers.clear();
173			m_outputSignals.clear();
174			}
175
176			/* --- PROTECTED MEMBER METHODS
177			* ---------------------------------------------------------- */
178
179			/* ------------------------------------------------------------------- */
180			/* --- ENTITY -------------------------------------------------------- */
181			/* ------------------------------------------------------------------- */
182
183			void TracePlayer::display(std::ostream& os) const {
184			os << "TracePlayer " << getName();
185			try {
186			getProfiler().report_all(3, os);
187			} catch (ExceptionSignal e) {
188			}
189			}
190			} // namespace torque_control
191			} // namespace sot
192			} // namespace dynamicgraph