1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __sot_torque_control_trace_player_H__ |
7 |
|
|
#define __sot_torque_control_trace_player_H__ |
8 |
|
|
|
9 |
|
|
/* --------------------------------------------------------------------- */ |
10 |
|
|
/* --- API ------------------------------------------------------------- */ |
11 |
|
|
/* --------------------------------------------------------------------- */ |
12 |
|
|
|
13 |
|
|
#if defined(WIN32) |
14 |
|
|
#if defined(__sot_torque_control_trace_player_H__) |
15 |
|
|
#define SOTTRACEPLAYER_EXPORT __declspec(dllexport) |
16 |
|
|
#else |
17 |
|
|
#define SOTTRACEPLAYER_EXPORT __declspec(dllimport) |
18 |
|
|
#endif |
19 |
|
|
#else |
20 |
|
|
#define SOTTRACEPLAYER_EXPORT |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
/* --------------------------------------------------------------------- */ |
24 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
|
27 |
|
|
#include <boost/assign.hpp> |
28 |
|
|
#include <map> |
29 |
|
|
#include <pinocchio/fwd.hpp> |
30 |
|
|
#include <vector> |
31 |
|
|
|
32 |
|
|
/* HELPER */ |
33 |
|
|
#include <dynamic-graph/signal-helper.h> |
34 |
|
|
|
35 |
|
|
#include <sot/core/matrix-geometry.hh> |
36 |
|
|
#include <sot/core/robot-utils.hh> |
37 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
38 |
|
|
|
39 |
|
|
namespace dynamicgraph { |
40 |
|
|
namespace sot { |
41 |
|
|
namespace torque_control { |
42 |
|
|
|
43 |
|
|
/* --------------------------------------------------------------------- */ |
44 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
45 |
|
|
/* --------------------------------------------------------------------- */ |
46 |
|
|
|
47 |
|
|
/** |
48 |
|
|
* @brief Entity to play data saved using a Tracer. |
49 |
|
|
* |
50 |
|
|
* A typical use of this entity would be to call the command |
51 |
|
|
* addOutputSignal for every file you previously saved with |
52 |
|
|
* the Tracer. Then you can either call the command |
53 |
|
|
* playNext, or you can call recompute on the output |
54 |
|
|
* signal "trigger". |
55 |
|
|
*/ |
56 |
|
|
class SOTTRACEPLAYER_EXPORT TracePlayer : public ::dynamicgraph::Entity { |
57 |
|
|
typedef TracePlayer EntityClassName; |
58 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
59 |
|
|
|
60 |
|
|
public: |
61 |
|
|
/* --- CONSTRUCTOR ---- */ |
62 |
|
|
TracePlayer(const std::string& name); |
63 |
|
|
|
64 |
|
|
void init(const double& dt); |
65 |
|
|
|
66 |
|
|
/* --- SIGNALS --- */ |
67 |
|
|
typedef dynamicgraph::Signal<dynamicgraph::Vector, int> OutputSignalType; |
68 |
|
|
std::map<std::string, OutputSignalType*> m_outputSignals; |
69 |
|
|
DECLARE_SIGNAL_OUT(trigger, int); |
70 |
|
|
|
71 |
|
|
/* --- COMMANDS --- */ |
72 |
|
|
void addOutputSignal(const std::string& fileName, |
73 |
|
|
const std::string& signalName); |
74 |
|
|
void playNext(); |
75 |
|
|
void rewind(); |
76 |
|
|
void clear(); |
77 |
|
|
|
78 |
|
|
/* --- ENTITY INHERITANCE --- */ |
79 |
|
|
virtual void display(std::ostream& os) const; |
80 |
|
|
|
81 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
82 |
|
|
const char* = "", int = 0) { |
83 |
|
|
logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
protected: |
87 |
|
|
typedef dynamicgraph::Vector DataType; |
88 |
|
|
typedef std::list<DataType> DataHistoryType; |
89 |
|
|
typedef DataHistoryType::const_iterator DataPointerType; |
90 |
|
|
|
91 |
|
|
std::map<std::string, DataHistoryType> m_data; |
92 |
|
|
std::map<std::string, DataPointerType> m_dataPointers; |
93 |
|
|
|
94 |
|
|
}; // class TraceReader |
95 |
|
|
|
96 |
|
|
} // namespace torque_control |
97 |
|
|
} // namespace sot |
98 |
|
|
} // namespace dynamicgraph |
99 |
|
|
|
100 |
|
|
#endif // #ifndef __sot_torque_control_trace_reader_H__ |