| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/** |
||
2 |
* Copyright 2019 Max Planck Society. All rights reserved. |
||
3 |
* Julian Viereck |
||
4 |
*/ |
||
5 |
|||
6 |
#include "dg_tools/operator.hpp" |
||
7 |
|||
8 |
/* --------------------------------------------------------------------- */ |
||
9 |
/* --------------------------------------------------------------------- */ |
||
10 |
/* --------------------------------------------------------------------- */ |
||
11 |
#include <sot/core/debug.hh> |
||
12 |
#include <dynamic-graph/factory.h> |
||
13 |
#include <dynamic-graph/all-commands.h> |
||
14 |
|||
15 |
#include <iostream> |
||
16 |
|||
17 |
using namespace dg_tools; |
||
18 |
|||
19 |
✓✗✓✗ ✓✗✓✗ |
4 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PoseQuaternionToPoseRPY, "PoseQuaternionToPoseRPY"); |
20 |
|||
21 |
/* --------------------------------------------------------------------- */ |
||
22 |
/* --------------------------------------------------------------------- */ |
||
23 |
/* --------------------------------------------------------------------- */ |
||
24 |
|||
25 |
2 |
PoseQuaternionToPoseRPY::PoseQuaternionToPoseRPY( const std::string & name ) |
|
26 |
:Entity(name) |
||
27 |
✓✗✓✗ |
4 |
,data_inputSIN(NULL,"PoseQuaternionToPoseRPY("+name+")::input(vector)::sin") |
28 |
,data_outSOUT( boost::bind(&PoseQuaternionToPoseRPY::data_out_callback,this,_1,_2), |
||
29 |
data_inputSIN, |
||
30 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
"PoseQuaternionToPoseRPY("+name+")::output(vector)::sout" ) |
31 |
{ |
||
32 |
✓✗✓✗ |
2 |
Entity::signalRegistration(data_inputSIN << data_outSOUT); |
33 |
2 |
} |
|
34 |
|||
35 |
/* --------------------------------------------------------------------- */ |
||
36 |
/* --------------------------------------------------------------------- */ |
||
37 |
/* --------------------------------------------------------------------- */ |
||
38 |
|||
39 |
2 |
dg::Vector& PoseQuaternionToPoseRPY::data_out_callback(dg::Vector& out, int time) |
|
40 |
{ |
||
41 |
✓✗ | 2 |
const dg::Vector& input = data_inputSIN(time); |
42 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
dynamicgraph::sot::VectorQuaternion quat(input(6), input(3), input(4), input(5)); |
43 |
|||
44 |
✓✗ | 2 |
out.resize(6); |
45 |
✓✗✓✗ ✓✗ |
2 |
out.head<3>() = input.head<3>(); |
46 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
out.tail<3>() = quat.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); |
47 |
2 |
return out; |
|
48 |
} |
||
49 |
|||
50 |
✓✗✓✗ ✓✗✗✗ |
2 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sinus, "Sinus"); |
51 |
|||
52 |
/* --------------------------------------------------------------------- */ |
||
53 |
/* --------------------------------------------------------------------- */ |
||
54 |
/* --------------------------------------------------------------------- */ |
||
55 |
|||
56 |
Sinus::Sinus( const std::string & name ) |
||
57 |
:Entity(name) |
||
58 |
,data_inputSIN(NULL,"Sinus("+name+")::input(double)::sin") |
||
59 |
,data_outSOUT( boost::bind(&Sinus::data_out_callback,this,_1,_2), |
||
60 |
data_inputSIN, |
||
61 |
"Sinus("+name+")::output(double)::sout" ) |
||
62 |
{ |
||
63 |
Entity::signalRegistration(data_inputSIN << data_outSOUT); |
||
64 |
} |
||
65 |
|||
66 |
/* --------------------------------------------------------------------- */ |
||
67 |
/* --------------------------------------------------------------------- */ |
||
68 |
/* --------------------------------------------------------------------- */ |
||
69 |
|||
70 |
double& Sinus::data_out_callback(double& out, int time) |
||
71 |
{ |
||
72 |
out = sin(data_inputSIN.access(time)); |
||
73 |
return out; |
||
74 |
} |
||
75 |
|||
76 |
|||
77 |
✓✗✓✗ ✓✗✗✗ |
2 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Division_of_double, "Division_of_double"); |
78 |
|||
79 |
/* --------------------------------------------------------------------- */ |
||
80 |
/* --------------------------------------------------------------------- */ |
||
81 |
/* --------------------------------------------------------------------- */ |
||
82 |
|||
83 |
Division_of_double::Division_of_double( const std::string & name ) |
||
84 |
:Entity(name) |
||
85 |
,data_input1SIN(NULL,"Division_of_double("+name+")::input(double)::sin1") |
||
86 |
,data_input2SIN(NULL,"Division_of_double("+name+")::input(double)::sin2") |
||
87 |
,data_outSOUT( boost::bind(&Division_of_double::data_out_callback,this,_1,_2), |
||
88 |
data_input1SIN << data_input2SIN, |
||
89 |
"Division_of_double("+name+")::output(double)::sout" ) |
||
90 |
{ |
||
91 |
Entity::signalRegistration(data_input1SIN << data_input2SIN << data_outSOUT); |
||
92 |
} |
||
93 |
|||
94 |
/* --------------------------------------------------------------------- */ |
||
95 |
/* --------------------------------------------------------------------- */ |
||
96 |
/* --------------------------------------------------------------------- */ |
||
97 |
|||
98 |
double& Division_of_double::data_out_callback(double& out, int time) |
||
99 |
{ |
||
100 |
out = data_input1SIN.access(time) / data_input2SIN.access(time); |
||
101 |
return out; |
||
102 |
} |
||
103 |
|||
104 |
|||
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ |
3 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorIntegrator, "VectorIntegrator"); |
107 |
|||
108 |
/* --------------------------------------------------------------------- */ |
||
109 |
/* --------------------------------------------------------------------- */ |
||
110 |
/* --------------------------------------------------------------------- */ |
||
111 |
|||
112 |
1 |
VectorIntegrator::VectorIntegrator( const std::string & name ) |
|
113 |
:Entity(name) |
||
114 |
,init_(true) |
||
115 |
✓✗✓✗ |
2 |
,data_inputSIN(NULL,"VectorIntegrator("+name+")::input(vector)::sin") |
116 |
,data_outSOUT( boost::bind(&VectorIntegrator::data_out_callback,this,_1,_2), |
||
117 |
data_inputSIN, |
||
118 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
"VectorIntegrator("+name+")::output(vector)::sout" ) |
119 |
{ |
||
120 |
✓✗✓✗ |
1 |
Entity::signalRegistration(data_inputSIN << data_outSOUT); |
121 |
|||
122 |
1 |
data_outSOUT.setDependencyType( |
|
123 |
1 |
dynamicgraph::TimeDependency<int>::ALWAYS_READY); |
|
124 |
1 |
} |
|
125 |
|||
126 |
/* --------------------------------------------------------------------- */ |
||
127 |
/* --------------------------------------------------------------------- */ |
||
128 |
/* --------------------------------------------------------------------- */ |
||
129 |
|||
130 |
3 |
dg::Vector& VectorIntegrator::data_out_callback(dg::Vector& out, int time) |
|
131 |
{ |
||
132 |
3 |
const dg::Vector& input = data_inputSIN(time); |
|
133 |
3 |
out.resize(input.size()); |
|
134 |
✓✓ | 3 |
if (init_) { |
135 |
1 |
init_ = false; |
|
136 |
1 |
sum_.resize(input.size()); |
|
137 |
1 |
sum_.setZero(input.size()); |
|
138 |
} |
||
139 |
|||
140 |
// TODO: Avoid hard-coded sampling rate here. |
||
141 |
✓✗✓✗ |
3 |
sum_ += 0.001 * input; |
142 |
3 |
out = sum_; |
|
143 |
3 |
return out; |
|
144 |
✓✗✓✗ |
6 |
} |
| Generated by: GCOVR (Version 4.2) |