sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
fir-filter.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_FIRFILTER_HH__
11 #define __SOT_FIRFILTER_HH__
12 
13 #include <cassert>
14 
15 #include <algorithm>
16 #include <iterator>
17 #include <vector>
18 
19 #include <dynamic-graph/all-signals.h>
20 #include <dynamic-graph/command-getter.h>
21 #include <dynamic-graph/command-setter.h>
22 #include <dynamic-graph/entity.h>
23 
24 namespace dynamicgraph {
25 namespace sot {
26 
27 namespace detail {
28 // GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
29 // As a workaround, only the part of circular_buffer's interface used
30 // here is implemented.
31 // Ugly, fatty piece of code.
32 template <class T> class circular_buffer {
33 public:
34  circular_buffer() : buf(1), start(0), numel(0) {}
35  void push_front(const T &data) {
36  if (start) {
37  --start;
38  } else {
39  start = buf.size() - 1;
40  }
41  buf[start] = data;
42  if (numel < buf.size()) {
43  ++numel;
44  }
45  }
46  void reset_capacity(size_t n) {
47  buf.resize(n);
48  start = 0;
49  numel = 0;
50  }
51  void reset_capacity(size_t n, const T &el) {
52  buf.clear();
53  buf.resize(n, el);
54  start = 0;
55  numel = 0;
56  }
57  T &operator[](size_t i) {
58  assert((i < numel) && "Youre accessing an empty buffer");
59  size_t index = (start + i) % buf.size();
60  return buf[index];
61  }
62  size_t size() const { return numel; }
63 
64 private:
65  std::vector<T> buf;
66  size_t start;
67  size_t numel;
68 }; // class circular_buffer
69 } // namespace detail
70 
71 template <class sigT, class coefT> class FIRFilter;
72 
73 namespace command {
74 using ::dynamicgraph::command::Command;
75 using ::dynamicgraph::command::Value;
76 
77 template <class sigT, class coefT> class SetElement : public Command {
78 public:
79  SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
80  Value doExecute();
81 }; // class SetElement
82 
83 template <class sigT, class coefT> class GetElement : public Command {
84 public:
85  GetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
86  Value doExecute();
87 }; // class SetElement
88 } // namespace command
89 
90 using ::dynamicgraph::command::Getter;
91 using ::dynamicgraph::command::Setter;
92 
93 template <class sigT, class coefT> class FIRFilter : public Entity {
94 public:
95  virtual const std::string &getClassName() const {
96  return Entity::getClassName();
97  }
98  static std::string getTypeName(void) { return "Unknown"; }
99  static const std::string CLASS_NAME;
100 
101  std::string getDocString() const {
102  return "Finite impulse response filter\n"
103  "\n"
104  " Provide the following sum in output signal:\n"
105  " N \n"
106  " __ \n"
107  " y (n) = \\ c s (n-i) \n"
108  " /_ i \n"
109  " i=0 \n"
110  " \n"
111  " where\n"
112  " - c_i are coefficients stored in an array\n"
113  " - N is the size of the array\n"
114  " - s is the input signal.\n";
115  }
116 
117 public:
118  FIRFilter(const std::string &name)
119  : Entity(name), SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
120  SOUT(boost::bind(&FIRFilter::compute, this, _1, _2), SIN,
121  "sotFIRFilter(" + name + ")::output(T)::sout") {
122  signalRegistration(SIN << SOUT);
123  std::string docstring = " Set element at rank in array of coefficients\n"
124  "\n"
125  " Input:\n"
126  " - positive int: rank\n"
127  " - element\n";
128  addCommand("setElement",
129  new command::SetElement<sigT, coefT>(*this, docstring));
130  docstring = " Get element at rank in array of coefficients\n"
131  "\n"
132  " Input:\n"
133  " - positive int: rank\n"
134  " Return:\n"
135  " - element\n";
136  addCommand("getElement",
137  new command::GetElement<sigT, coefT>(*this, docstring));
138  docstring = " Set number of coefficients\n"
139  "\n"
140  " Input:\n"
141  " - positive int: size\n";
142  addCommand("setSize", new Setter<FIRFilter, unsigned>(
143  *this, &FIRFilter::resizeBuffer, docstring));
144 
145  docstring = " Get Number of coefficients\n"
146  "\n"
147  " Return:\n"
148  " - positive int: size\n";
149  addCommand("getSize", new Getter<FIRFilter, unsigned>(
150  *this, &FIRFilter::getBufferSize, docstring));
151  }
152 
153  virtual ~FIRFilter() {}
154 
155  virtual sigT &compute(sigT &res, int time) {
156  const sigT &in = SIN.access(time);
157  reset_signal(res, in);
158  data.push_front(in);
159 
160  size_t SIZE = std::min(data.size(), coefs.size());
161  for (size_t i = 0; i < SIZE; ++i) {
162  res += coefs[i] * data[i];
163  }
164 
165  return res;
166  }
167 
168  void resizeBuffer(const unsigned int &size) {
169  size_t s = static_cast<size_t>(size);
170  data.reset_capacity(s);
171  coefs.resize(s);
172  }
173 
174  unsigned int getBufferSize() const {
175  return static_cast<unsigned int>(coefs.size());
176  }
177 
178  void setElement(const unsigned int &rank, const coefT &coef) {
179  coefs[rank] = coef;
180  }
181 
182  coefT getElement(const unsigned int &rank) const { return coefs[rank]; }
183 
184  static void reset_signal(sigT & /*res*/, const sigT & /*sample*/) {}
185 
186 public:
187  SignalPtr<sigT, int> SIN;
188  SignalTimeDependent<sigT, int> SOUT;
189 
190 private:
191  std::vector<coefT> coefs;
193 }; // class FIRFilter
194 
195 namespace command {
196 using ::dynamicgraph::command::Command;
197 using ::dynamicgraph::command::Value;
198 using ::dynamicgraph::command::ValueHelper;
199 
200 template <class sigT, class coefT>
202  const std::string &docstring)
203  : Command(
204  entity,
205  boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
206  docstring) {}
207 
208 template <class sigT, class coefT> Value SetElement<sigT, coefT>::doExecute() {
209  FIRFilter<sigT, coefT> &entity =
210  static_cast<FIRFilter<sigT, coefT> &>(owner());
211  std::vector<Value> values = getParameterValues();
212  unsigned int rank = values[0].value();
213  coefT coef = values[1].value();
214  entity.setElement(rank, coef);
215  return Value();
216 }
217 
218 template <class sigT, class coefT>
220  const std::string &docstring)
221  : Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}
222 
223 template <class sigT, class coefT> Value GetElement<sigT, coefT>::doExecute() {
224  FIRFilter<sigT, coefT> &entity =
225  static_cast<FIRFilter<sigT, coefT> &>(owner());
226  std::vector<Value> values = getParameterValues();
227  unsigned int rank = values[0].value();
228  return Value(entity.getElement(rank));
229 }
230 } // namespace command
231 
232 } // namespace sot
233 } // namespace dynamicgraph
234 
235 #endif
coefT getElement(const unsigned int &rank) const
Definition: fir-filter.hh:182
virtual sigT & compute(sigT &res, int time)
Definition: fir-filter.hh:155
Value doExecute()
Definition: fir-filter.hh:208
unsigned int getBufferSize() const
Definition: fir-filter.hh:174
size_t size() const
Definition: fir-filter.hh:62
virtual const std::string & getClassName() const
Definition: fir-filter.hh:95
Definition: fir-filter.hh:77
FIRFilter(const std::string &name)
Definition: fir-filter.hh:118
Definition: fir-filter.hh:83
SignalPtr< sigT, int > SIN
Definition: fir-filter.hh:187
void resizeBuffer(const unsigned int &size)
Definition: fir-filter.hh:168
static void reset_signal(sigT &, const sigT &)
Definition: fir-filter.hh:184
void push_front(const T &data)
Definition: fir-filter.hh:35
void reset_capacity(size_t n)
Definition: fir-filter.hh:46
static std::string getTypeName(void)
Definition: fir-filter.hh:98
Definition: fir-filter.hh:32
void reset_capacity(size_t n, const T &el)
Definition: fir-filter.hh:51
circular_buffer()
Definition: fir-filter.hh:34
Definition: fir-filter.hh:71
virtual ~FIRFilter()
Definition: fir-filter.hh:153
SignalTimeDependent< sigT, int > SOUT
Definition: fir-filter.hh:188
static const std::string CLASS_NAME
Definition: fir-filter.hh:99
void setElement(const unsigned int &rank, const coefT &coef)
Definition: fir-filter.hh:178
GetElement(FIRFilter< sigT, coefT > &entity, const std::string &docstring)
Definition: fir-filter.hh:219
std::string getDocString() const
Definition: fir-filter.hh:101
Value doExecute()
Definition: fir-filter.hh:223
Definition: abstract-sot-external-interface.hh:17
T & operator[](size_t i)
Definition: fir-filter.hh:57