-
Notifications
You must be signed in to change notification settings - Fork 0
/
adap_samples_input.orogen
312 lines (212 loc) · 11.3 KB
/
adap_samples_input.orogen
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
name "adap_samples_input"
# version "0.1"
using_library "adap_samples_input"
using_task_library 'uwv_motion_model'
using_task_library 'ls_pseudoinverse'
import_types_from "base"
import_types_from "adap_samples_input/samples_dataType.h"
task_context "Task" do
# sample time of pose_sample
property "step", "double", 0.01
# delay of the thruster data and pose_samples
property "delay", "double", 0.0
# Savitzky-Golay filter.
property "filter_sav_gol", "bool", true
# Velocity avalaible, compute acceleration. If false, pose avalaible, compute velocity and acceleration
property "velocity_avalible", "bool", true
# Smooth given data (position or velocity).
property "smooth", "bool", false
# degree of polynomial for filter, to get acceleration
property "poly", "double", 3
# Position of the computed derivatives. from -halfSize to +halfSize
property "pos_filter", "double", 0
# half number of samples used to compute derivatives
property "number_samples", "double", 10.0
input_port('pose_samples', '/base/samples/RigidBodyState').
doc("current pose, can not have velocities data ")
input_port('forces_samples', '/base/samples/Joints').
doc("forces applied in auv")
output_port('velocities', '/base/samples/RigidBodyState').
doc("Velocities in auv, in body-frame")
output_port('forces', '/base/samples/Joints').
doc("forces applied in the auv")
output_port('dynamic', 'adap_samples_input::DynamicAUV').
doc("agglomerate forces, velocities and accelerations data in body-frame")
#******************************
#** Aggregator Parameters *****
#******************************
stream_aligner do
align_port("pose_samples", 0.001)
align_port("forces_samples", 0.001)
max_latency(0.1)
end
periodic 0.01
end
task_context "Seabotix" do
# sample time of pose_sample
property "step", "double", 0.01
# delay of the thruster data
property "delay", "double", 1.0
# half number of samples used to compute derivatives
property "number_samples", "double", 10.0
input_port('position_samples', '/base/samples/RigidBodyState').
doc("current position given by detector.pose //camera ")
input_port('forces_samples', '/base/samples/Joints').
doc("input pwm signal applied in the thrusters")
output_port('velocity', '/base/samples/RigidBodyState').
doc("velocity after filter and derivated the position_samples")
output_port('acceleration', '/base/samples/RigidBodyAcceleration').
doc("accelaration after filter and 2th derivated the position_samples")
output_port('forces', '/base/samples/Joints').
doc("forces applied in the auv. Convert the pwm input into force and applied the thruster matrix")
output_port('dynamic', 'adap_samples_input::DynamicAUV').
doc("agglomerate all data of the other ports (force, rbs and rba) ")
port_driven
end
task_context "Dagon" do
# If true, data will be aligned (use for least squarer method). If false, the data will be trasmited direct after treatment.
property "aligned_data", "bool", true
# If true, use the velocitu data from pose_estimator. If false use the data from DVL and orientation_estimation
property "pose_estimator_data", "bool", true
# sample time of pose_sample
property "step", "double", 0.01
# convert thruster rpm into force or surge. [0]:posX, [1]:negX, [2]:posY/Z, [3]:negY/Z
property "Cv", "base::VectorXd"
# matrix of trhuster. identifie the forces and torques aplied for each of n trhuster
property "TCM", "base::MatrixXd"
## Use case pose_estimator_data is false
input_port('dvl_samples', '/base/samples/RigidBodyState').
doc("current velocity given by dvl")
## Use case pose_estimator_data is false
input_port('orientation_samples', '/base/samples/RigidBodyState').
doc("current orientation given by orientation estimation (imu/fog)")
## Use case pose_estimator_data is true
input_port('position_samples', '/base/samples/RigidBodyState').
doc("current position given by pose_estimation (dvl and imu/fog) ")
input_port('forces_samples', '/base/samples/Joints').
doc("angular_velocity of thrusters in rpm (V)")
output_port('velocity', '/base/samples/RigidBodyState').
doc("velocity after filter. Used for alligned porpose with acceleration")
output_port('lin_acceleration', '/base/samples/RigidBodyAcceleration').
doc("linear accelaration after derivated the velocity_samples")
output_port('ang_acceleration', '/base/samples/RigidBodyAcceleration').
doc("angular accelaration after derivated the velocity_samples")
output_port('forces', '/base/samples/Joints').
doc("forces applied in the auv. Convert the rpm into force and applied the thruster matrix")
output_port('dynamic', 'adap_samples_input::DynamicAUV').
doc("agglomerate all data of the other ports (force, rbs and lin_rba, ang_rba) alligned ")
port_driven
end
task_context "ForceApplier" do
needs_configuration
# The amount of actuators the vehicle has
property "number_of_thruster", "int"
# Convert thruster signal into forces, in positive direction. Should have the size of amount_of_actuators
property "thruster_coefficients_Pos", "base::VectorXd"
# Convert thruster signal into forces, in negative direction. Should have the size of amount_of_actuators
property "thruster_coefficients_Neg", "base::VectorXd"
# Matrix of trhuster. Identifie the forces and torques aplied for each of n trhuster
property "TCM", "base::MatrixXd"
# If left empty, uses RAW by default
property "control_modes", "std::vector<base::JointState::MODE>"
# In case the control mode is PWM, used to convert the signal into DC Voltage
property "thrusterVoltage", "double", 0
# Names of the thrusters
# Leave empty to use no names
property "names", "std::vector<std::string>"
# If true, will not write the output. The forces may be treated in other task if ForceApplier is used as subclasses
property "treatOutput", "bool", false
## Thrusters signal
input_port('thruster_samples', '/base/samples/Joints').
doc("thrusters signal")
## Forces and torques applied in the vehicle in body-frame
output_port('forces', '/base/samples/Joints').
doc("Forces applied in the auv. Convert the input signal into force and applied the thruster matrix.")
## Forces and signals applied for each thruster in the vehicle
output_port('thruster_forces', '/base/samples/Joints').
doc("Forces and signals applied by each thruster in the auv.")
exception_states :WRONG_SIZE_OF_CONTROLMODES, :WRONG_SIZE_OF_THRUSTER_COEFFICIENTS, :WRONG_SIZE_OF_NAMES,
:WRONG_SIZE_OF_THRUSTER_MATRIX, :UNSET_THRUSTER_INPUT, :UNEXPECTED_THRUSTER_INPUT
port_driven
end
task_context "GetPoseForce" do
subclasses "ForceApplier"
# If true, data will be aligned (use for least square method). If false, the data will be trasmited direct after treatment.
property "aligned_data", "bool", true
# sample time of pose_sample
property "step", "double", 0.01
## Pose samples in world-frame
input_port('pose_samples', '/base/samples/RigidBodyState').
doc("current position given by pose_estimation (dvl and imu/fog) ")
## Outputs of velocities, acceleration, forces and torques
output_port('velocity', '/base/samples/RigidBodyState').
doc("velocity after filter. Used for alligned porpose with acceleration")
output_port('lin_acceleration', '/base/samples/RigidBodyAcceleration').
doc("linear accelaration after derivated the velocity_samples")
output_port('ang_acceleration', '/base/samples/RigidBodyAcceleration').
doc("angular accelaration after derivated the velocity_samples")
output_port('dynamic', 'adap_samples_input::DynamicAUV').
doc("agglomerate all data of the other ports (force, rbs and lin_rba, ang_rba) alligned ")
port_driven
end
task_context "GetPoseForcePeriodic" do
#subclasses "ForceApplier"
# If true, data will be aligned and transmitted (use for adaptive method). If false, acceleration will be computed, with delay (use for least square method).
# property "adap_method", "bool", true
# sample time
property "step", "double", 0.1
# degree of polynomial for filter, to get acceleration
property "poly", "double", 3
# Half minus one amount of data used to compute the acceleration
property "halfSize", "int", 50
# Position of the computed acceleration. from -halfSize to +halfSize
property "posFilter", "double", 0
## Pose samples in world-frame
input_port('pose_samples', '/base/samples/RigidBodyState').
doc("current position given by pose_estimation (dvl and imu/fog) ")
## Thrusters signal
input_port('forces_samples', '/base/samples/Joints').
doc("Forces and torques applied in the vehicle")
## Outputs of velocities, acceleration, forces and torques
output_port('velocity', '/base/samples/RigidBodyState').
doc("velocity after filter. Used for alligned porpose with acceleration")
output_port('effort', '/base/samples/Joints').
doc("Forces and torques applied in the vehicle")
#output_port('lin_acceleration', '/base/samples/RigidBodyAcceleration').
# doc("linear accelaration after derivated the velocity_samples")
#output_port('ang_acceleration', '/base/samples/RigidBodyAcceleration').
# doc("angular accelaration after derivated the velocity_samples")
output_port('dynamic', 'adap_samples_input::DynamicAUV').
doc("agglomerate all data of the other ports (force, rbs and lin_rba, ang_rba) alligned ")
#******************************
#** Aggregator Parameters *****
#******************************
stream_aligner do
align_port("pose_samples", 0.01)
align_port("forces_samples", 0.01)
max_latency(0.1)
end
periodic 0.01
end
deployment "motion_model1" do
task 'motion_model1', 'uwv_motion_model::Task'
add_default_logger
end
deployment "motion_model2" do
task 'motion_model2', 'uwv_motion_model::Task'
add_default_logger
end
deployment "adap_samples_pose" do
task 'adap_samples_pose', 'adap_samples_input::Task'
task 'ls_method_pose', 'ls_pseudoinverse::Task'
add_default_logger
end
deployment "adap_samples_vel" do
task 'adap_samples_vel', 'adap_samples_input::Task'
task 'ls_method_vel', 'ls_pseudoinverse::Task'
add_default_logger
end
#deployment "motion_model2" do
# task 'motion_model2', 'uwv_motion_model::Task'
# add_default_logger
#end