Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/fir-filter.hh	Lines:	0	70	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	110	0.0 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_FIRFILTER_HH__
#define __SOT_FIRFILTER_HH__

#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/entity.h>

#include <algorithm>
#include <cassert>
#include <iterator>
#include <vector>

namespace dynamicgraph {
namespace sot {

namespace detail {
// GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
// As a workaround, only the part of circular_buffer's interface used
// here is implemented.
// Ugly, fatty piece of code.
template <class T>
class circular_buffer {
 public:
  circular_buffer() : buf(1), start(0), numel(0) {}
  void push_front(const T &data) {
    if (start) {
      --start;
    } else {
      start = buf.size() - 1;
    }
    buf[start] = data;
    if (numel < buf.size()) {
      ++numel;
    }
  }
  void reset_capacity(size_t n) {
    buf.resize(n);
    start = 0;
    numel = 0;
  }
  void reset_capacity(size_t n, const T &el) {
    buf.clear();
    buf.resize(n, el);
    start = 0;
    numel = 0;
  }
  T &operator[](size_t i) {
    assert((i < numel) && "Youre accessing an empty buffer");
    size_t index = (start + i) % buf.size();
    return buf[index];
  }
  size_t size() const { return numel; }

 private:
  std::vector<T> buf;
  size_t start;
  size_t numel;
};  // class circular_buffer
}  // namespace detail

template <class sigT, class coefT>
class FIRFilter;

namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;

template <class sigT, class coefT>
class SetElement : public Command {
 public:
  SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
  Value doExecute();
};  // class SetElement

template <class sigT, class coefT>
class GetElement : public Command {
 public:
  GetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
  Value doExecute();
};  // class SetElement
}  // namespace command

using ::dynamicgraph::command::Getter;
using ::dynamicgraph::command::Setter;

template <class sigT, class coefT>
class FIRFilter : public Entity {
 public:
  virtual const std::string &getClassName() const {
    return Entity::getClassName();
  }
  static std::string getTypeName(void) { return "Unknown"; }
  static const std::string CLASS_NAME;

  std::string getDocString() const {
    return "Finite impulse response filter\n"
           "\n"
           "  Provide the following sum in output signal:\n"
           "             N                   \n"
           "             __                  \n"
           "     y (n) = \\   c  s (n-i)      \n"
           "             /_   i              \n"
           "             i=0                 \n"
           "                                 \n"
           "  where\n"
           "    -  c_i are coefficients stored in an array\n"
           "    -  N is the size of the array\n"
           "    -  s is the input signal.\n";
  }

 public:
  FIRFilter(const std::string &name)
      : Entity(name),
        SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
        SOUT(boost::bind(&FIRFilter::compute, this, _1, _2), SIN,
             "sotFIRFilter(" + name + ")::output(T)::sout") {
    signalRegistration(SIN << SOUT);
    std::string docstring =
        "  Set element at rank in array of coefficients\n"
        "\n"
        "    Input:\n"
        "      - positive int: rank\n"
        "      - element\n";
    addCommand("setElement",
               new command::SetElement<sigT, coefT>(*this, docstring));
    docstring =
        "  Get element at rank in array of coefficients\n"
        "\n"
        "    Input:\n"
        "      - positive int: rank\n"
        "    Return:\n"
        "      - element\n";
    addCommand("getElement",
               new command::GetElement<sigT, coefT>(*this, docstring));
    docstring =
        "  Set number of coefficients\n"
        "\n"
        "    Input:\n"
        "      - positive int: size\n";
    addCommand("setSize", new Setter<FIRFilter, unsigned>(
                              *this, &FIRFilter::resizeBuffer, docstring));

    docstring =
        "  Get Number of coefficients\n"
        "\n"
        "    Return:\n"
        "      - positive int: size\n";
    addCommand("getSize", new Getter<FIRFilter, unsigned>(
                              *this, &FIRFilter::getBufferSize, docstring));
  }

  virtual ~FIRFilter() {}

  virtual sigT &compute(sigT &res, int time) {
    const sigT &in = SIN.access(time);
    reset_signal(res, in);
    data.push_front(in);

    size_t SIZE = std::min(data.size(), coefs.size());
    for (size_t i = 0; i < SIZE; ++i) {
      res += coefs[i] * data[i];
    }

    return res;
  }

  void resizeBuffer(const unsigned int &size) {
    size_t s = static_cast<size_t>(size);
    data.reset_capacity(s);
    coefs.resize(s);
  }

  unsigned int getBufferSize() const {
    return static_cast<unsigned int>(coefs.size());
  }

  void setElement(const unsigned int &rank, const coefT &coef) {
    coefs[rank] = coef;
  }

  coefT getElement(const unsigned int &rank) const { return coefs[rank]; }

  static void reset_signal(sigT & /*res*/, const sigT & /*sample*/) {}

 public:
  SignalPtr<sigT, int> SIN;
  SignalTimeDependent<sigT, int> SOUT;

 private:
  std::vector<coefT> coefs;
  detail::circular_buffer<sigT> data;
};  // class FIRFilter

namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
using ::dynamicgraph::command::ValueHelper;

template <class sigT, class coefT>
SetElement<sigT, coefT>::SetElement(FIRFilter<sigT, coefT> &entity,
                                    const std::string &docstring)
    : Command(
          entity,
          boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
          docstring) {}

template <class sigT, class coefT>
Value SetElement<sigT, coefT>::doExecute() {
  FIRFilter<sigT, coefT> &entity =
      static_cast<FIRFilter<sigT, coefT> &>(owner());
  std::vector<Value> values = getParameterValues();
  unsigned int rank = values[0].value();
  coefT coef = values[1].value();
  entity.setElement(rank, coef);
  return Value();
}

template <class sigT, class coefT>
GetElement<sigT, coefT>::GetElement(FIRFilter<sigT, coefT> &entity,
                                    const std::string &docstring)
    : Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}

template <class sigT, class coefT>
Value GetElement<sigT, coefT>::doExecute() {
  FIRFilter<sigT, coefT> &entity =
      static_cast<FIRFilter<sigT, coefT> &>(owner());
  std::vector<Value> values = getParameterValues();
  unsigned int rank = values[0].value();
  return Value(entity.getElement(rank));
}
}  // namespace command

}  // namespace sot
}  // namespace dynamicgraph

#endif


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
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 <dynamic-graph/all-signals.h>
14			#include <dynamic-graph/command-getter.h>
15			#include <dynamic-graph/command-setter.h>
16			#include <dynamic-graph/entity.h>
17
18			#include <algorithm>
19			#include <cassert>
20			#include <iterator>
21			#include <vector>
22
23			namespace dynamicgraph {
24			namespace sot {
25
26			namespace detail {
27			// GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
28			// As a workaround, only the part of circular_buffer's interface used
29			// here is implemented.
30			// Ugly, fatty piece of code.
31			template <class T>
32			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>
72			class FIRFilter;
73
74			namespace command {
75			using ::dynamicgraph::command::Command;
76			using ::dynamicgraph::command::Value;
77
78			template <class sigT, class coefT>
79			class SetElement : public Command {
80			public:
81			SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
82			Value doExecute();
83			}; // class SetElement
84
85			template <class sigT, class coefT>
86			class GetElement : public Command {
87			public:
88			GetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
89			Value doExecute();
90			}; // class SetElement
91			} // namespace command
92
93			using ::dynamicgraph::command::Getter;
94			using ::dynamicgraph::command::Setter;
95
96			template <class sigT, class coefT>
97			class FIRFilter : public Entity {
98			public:
99			virtual const std::string &getClassName() const {
100			return Entity::getClassName();
101			}
102			static std::string getTypeName(void) { return "Unknown"; }
103			static const std::string CLASS_NAME;
104
105			std::string getDocString() const {
106			return "Finite impulse response filter\n"
107			"\n"
108			" Provide the following sum in output signal:\n"
109			" N \n"
110			" __ \n"
111			" y (n) = \\ c s (n-i) \n"
112			" /_ i \n"
113			" i=0 \n"
114			" \n"
115			" where\n"
116			" - c_i are coefficients stored in an array\n"
117			" - N is the size of the array\n"
118			" - s is the input signal.\n";
119			}
120
121			public:
122			FIRFilter(const std::string &name)
123			: Entity(name),
124			SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
125			SOUT(boost::bind(&FIRFilter::compute, this, _1, _2), SIN,
126			"sotFIRFilter(" + name + ")::output(T)::sout") {
127			signalRegistration(SIN << SOUT);
128			std::string docstring =
129			" Set element at rank in array of coefficients\n"
130			"\n"
131			" Input:\n"
132			" - positive int: rank\n"
133			" - element\n";
134			addCommand("setElement",
135			new command::SetElement<sigT, coefT>(*this, docstring));
136			docstring =
137			" Get element at rank in array of coefficients\n"
138			"\n"
139			" Input:\n"
140			" - positive int: rank\n"
141			" Return:\n"
142			" - element\n";
143			addCommand("getElement",
144			new command::GetElement<sigT, coefT>(*this, docstring));
145			docstring =
146			" Set number of coefficients\n"
147			"\n"
148			" Input:\n"
149			" - positive int: size\n";
150			addCommand("setSize", new Setter<FIRFilter, unsigned>(
151			*this, &FIRFilter::resizeBuffer, docstring));
152
153			docstring =
154			" Get Number of coefficients\n"
155			"\n"
156			" Return:\n"
157			" - positive int: size\n";
158			addCommand("getSize", new Getter<FIRFilter, unsigned>(
159			*this, &FIRFilter::getBufferSize, docstring));
160			}
161
162			virtual ~FIRFilter() {}
163
164			virtual sigT &compute(sigT &res, int time) {
165			const sigT &in = SIN.access(time);
166			reset_signal(res, in);
167			data.push_front(in);
168
169			size_t SIZE = std::min(data.size(), coefs.size());
170			for (size_t i = 0; i < SIZE; ++i) {
171			res += coefs[i] * data[i];
172			}
173
174			return res;
175			}
176
177			void resizeBuffer(const unsigned int &size) {
178			size_t s = static_cast<size_t>(size);
179			data.reset_capacity(s);
180			coefs.resize(s);
181			}
182
183			unsigned int getBufferSize() const {
184			return static_cast<unsigned int>(coefs.size());
185			}
186
187			void setElement(const unsigned int &rank, const coefT &coef) {
188			coefs[rank] = coef;
189			}
190
191			coefT getElement(const unsigned int &rank) const { return coefs[rank]; }
192
193			static void reset_signal(sigT & /res/, const sigT & /sample/) {}
194
195			public:
196			SignalPtr<sigT, int> SIN;
197			SignalTimeDependent<sigT, int> SOUT;
198
199			private:
200			std::vector<coefT> coefs;
201			detail::circular_buffer<sigT> data;
202			}; // class FIRFilter
203
204			namespace command {
205			using ::dynamicgraph::command::Command;
206			using ::dynamicgraph::command::Value;
207			using ::dynamicgraph::command::ValueHelper;
208
209			template <class sigT, class coefT>
210			SetElement<sigT, coefT>::SetElement(FIRFilter<sigT, coefT> &entity,
211			const std::string &docstring)
212			: Command(
213			entity,
214			boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
215			docstring) {}
216
217			template <class sigT, class coefT>
218			Value SetElement<sigT, coefT>::doExecute() {
219			FIRFilter<sigT, coefT> &entity =
220			static_cast<FIRFilter<sigT, coefT> &>(owner());
221			std::vector<Value> values = getParameterValues();
222			unsigned int rank = values[0].value();
223			coefT coef = values[1].value();
224			entity.setElement(rank, coef);
225			return Value();
226			}
227
228			template <class sigT, class coefT>
229			GetElement<sigT, coefT>::GetElement(FIRFilter<sigT, coefT> &entity,
230			const std::string &docstring)
231			: Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}
232
233			template <class sigT, class coefT>
234			Value GetElement<sigT, coefT>::doExecute() {
235			FIRFilter<sigT, coefT> &entity =
236			static_cast<FIRFilter<sigT, coefT> &>(owner());
237			std::vector<Value> values = getParameterValues();
238			unsigned int rank = values[0].value();
239			return Value(entity.getElement(rank));
240			}
241			} // namespace command
242
243			} // namespace sot
244			} // namespace dynamicgraph
245
246			#endif