6 #include <tsid/utils/stop-watch.hpp> 7 #include <tsid/utils/statistics.hpp> 8 #include <dynamic-graph/factory.h> 9 #include <sot/core/debug.hh> 33 Entity::signalRegistration(m_triggerSOUT);
36 addCommand(
"addOutputSignal",
39 docCommandVoid2(
"Add a new output signal",
"Name of the text file where to read the data (string)",
40 "Name of the output signal (string)")));
42 addCommand(
"playNext",
45 addCommand(
"rewind", makeCommandVoid0(*
this, &
TracePlayer::rewind, docCommandVoid0(
"Rewind all the data.")));
47 addCommand(
"clear", makeCommandVoid0(*
this, &
TracePlayer::clear, docCommandVoid0(
"Clear all the output signals.")));
55 std::string astr = toString(iter);
65 return SEND_MSG(
"It already exists a signal with name " + signalName, MSG_TYPE_ERROR);
68 std::ifstream datafile(fileName.c_str());
69 if (datafile.fail())
return SEND_MSG(
"Error trying to read the file " + fileName, MSG_TYPE_ERROR);
71 const unsigned int SIZE = 1024;
73 std::vector<double> newline;
75 bool firstIter =
true;
76 std::size_t size = -1;
77 string fileNameShort = fileName.substr(1 + fileName.find_last_of(
"/"));
78 while (datafile.good()) {
79 datafile.getline(buffer, SIZE);
80 const std::size_t gcount = datafile.gcount();
82 return SEND_MSG(
"Read error: line " + toString(nbLines) +
" too long in file " + fileNameShort, MSG_TYPE_ERROR);
84 std::istringstream iss(buffer);
90 if (iss.fail())
break;
94 if (newline.size() > 0) {
96 size = newline.size();
97 else if (size != newline.size()) {
98 SEND_MSG(
"In file " + fileNameShort +
" nb of elements in each line changed from " + toString(size) +
" to " +
99 toString(newline.size()) +
" at line " + toString(nbLines),
101 size = newline.size();
103 m_data[signalName].push_back(Eigen::Map<Vector>(&newline[0], newline.size()));
108 "Finished reading " + toString(nbLines) +
" lines of " + toString(size) +
" elements from file " + fileNameShort,
114 new OutputSignalType(getClassName() +
"(" + getName() +
")::output(dynamicgraph::Vector)::" + signalName);
122 typedef std::map<std::string, OutputSignalType*>::iterator it_type;
124 const string& signalName = it->first;
129 if (dataPointer != dataSet.end()) ++dataPointer;
131 if (dataPointer == dataSet.end())
132 SEND_WARNING_STREAM_MSG(
"Reached end of dataset for signal " + signalName);
134 signal->setConstant(*dataPointer);
139 typedef std::map<std::string, DataPointerType>::iterator it_type;
141 const string& signalName = it->first;
144 dataPointer = dataSet.begin();
161 os <<
"TracePlayer " << getName();
163 getProfiler().report_all(3, os);
164 }
catch (ExceptionSignal e) {
virtual void display(std::ostream &os) const
dynamicgraph::Signal< dynamicgraph::Vector, int > OutputSignalType
Entity to play data saved using a Tracer.
std::map< std::string, DataPointerType > m_dataPointers
std::map< std::string, DataHistoryType > m_data
void addOutputSignal(const std::string &fileName, const std::string &signalName)
AdmittanceController EntityClassName
DataHistoryType::const_iterator DataPointerType
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
TracePlayer(const std::string &name)
std::map< std::string, OutputSignalType * > m_outputSignals
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
std::list< DataType > DataHistoryType