-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy paththreed_odometry.orogen
136 lines (107 loc) · 6.48 KB
/
threed_odometry.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
name "threed_odometry"
# Optionally declare the version number
# version "0.1"
# If new data types need to be defined, they have to be put in a separate C++
# header, and this header will be loaded here
using_library "threed_odometry" # 3D-Kinematics and motion model solvers
# If the component/deployments use data types that are defined in other oroGen
# projects, these projects should be imported there as well.
import_types_from "base" # IMU/Orientation and Joints samples
import_types_from "ThreedOdometryTypes.hpp" # Task configuration types
# Declare the Three Odometry class
# The component computes the robot pose based on
# a complete motion model.
# Robot joints positions are needed to compute
# the forward kinematics of robot chains.
# Angular and robot joints
# rates are needed to compute the movement.
#
#
# The corresponding C++ class can be edited in tasks/Task.hpp and
# tasks/Task.cpp, and will be put in the threed_odometry namespace.
task_context "Task" do
#******************************
#**** Odometry Properties *****
#******************************
property('urdf_file', '/std/string').
doc "path to the rover urdf file"
property('kinematic_model_type', 'threed_odometry/ModelType', :NUMERICAL).
doc 'Model type to use: ANALYTICAL or NUMERICAL approach.'+
'You must write :NUMERICAL in case you are using the Kinematics model from threed_odometry library.'
property('odometry_source_frame', '/std/string').doc 'Text to define the From Frame for the transformer (accumulated pose or Dead-Reckoning)'
property('odometry_target_frame', '/std/string').doc 'Text to define the To Frame for the transformer (accumulated pose or Dead-Reckoning)'
property('delta_odometry_source_frame', '/std/string').doc 'Text to define the From Frame for the transformer (delta pose)'
property('delta_odometry_target_frame', '/std/string').doc 'Text to define the To Frame for the transformer (delta pose)'
#******************************
#*** IIR Filter Properties ****
#******************************
property('iir_filter', 'threed_odometry/IIRCoefficients').
doc 'Configuration values to run the Infinite Impulse Response (IIR). Look in Types for further details.'
#******************************
#*** Orientation Noise Flag ***
#******************************
property('orientation_samples_noise_on', 'bool').
doc 'Set to true if desired orientation noise from orientation_samples input port.'+
"Otherwise it uses the motion model Leasts-Squares error to fill the uncertainty for the orientation."
#******************************
#*** Joints Properties ****
#******************************
property('contact_point_segments', 'std/vector<std/string>').
doc 'Segment names for the segment of the last contact point in the URDF file. This is the last element per each kinematic chain.'
property('contact_angle_segments', 'std/vector<std/string>').
doc 'Segment names for the segment of the last contact angle in the URDF file. Normally, all segments with the contact joint attached.'
property('all_joint_names', 'std/vector<std/string>').
doc 'All joints in the order needed by KDL to solve the forward kinematics. ONLY movable joints, meaning joints which are NOT "fixed" in URDF.'
property('slip_joint_names', 'std/vector<std/string>').
doc 'Joints in the model which are part of the slip model. Normally, the joints attached to the contact point segments.'
property('contact_joint_names', 'std/vector<std/string>').
doc 'Joints in the model which are part of the contact angle. Normally, the joints attached to the contact angle segments'
#******************************
#******* Input ports *********
#******************************
input_port('joints_samples', '/base/samples/Joints').
needs_reliable_connection.
doc 'timestamped joints samples (active and passive) providing odometry information.'+
'They have to provide position and speed. In case they do not originally provide both values'+
'Another component should perform this computation.'
input_port('orientation_samples', '/base/samples/RigidBodyState').
needs_reliable_connection.
doc 'provides timestamped orientation samples.'
input_port('weighting_samples', '/base/VectorXd').
needs_reliable_connection.
doc 'provides timestamped Weight information about the contact points.'+
'It defines which contact point is in contact with the ground using a Weighting Matrix Diagonal entry.'+
'In case no information is provided, it assumes all the points are equally in contact with the ground'+
'(support polygon or contact sensory information could be use to provide this matrix).'
####################################################################
# INFO AND RECOMMENDATIONS:
# world_frame is the zero absolute, tangential to an imaginary plane.
# navigation_frame is the zero relative (where threed_odometry starts).
# This Odometry tasks assumes zero starting pose at relative
# navigation_frame.
####################################################################
transformer do
transform "imu", "body" # imu in body in "Source in target" convention (Tbody_imu in frames convention)
align_port("joints_samples", 0.08)
align_port("orientation_samples", 0.08)
max_latency(1.0)
end
#******************************
#******* Output Ports *********
#******************************
output_port('pose_samples_out', '/base/samples/RigidBodyState').
doc 'Estimated robot pose. Position and velocity is computed from statistical Motion Model.'+
'Orientation from IMU gyros integration.'
output_port('delta_pose_samples_out', '/base/samples/RigidBodyState').
doc 'Delta estimated robot pose (from body_frame k-1). Position and velocity is computed from statistical Motion Model.'+
'Orientation from IMU gyros integration.'
#************************************
#******* Output Debug Ports *********
#************************************
property('output_debug', 'bool', false).
doc 'Set to true in case you want to have the output ports bellow.'
output_port('fkchains_rbs_out', 'threed_odometry/RobotContactPointsRbs').
doc 'Rigid Body States with the position and orientation of Contact points frame'+
'of the robot with respect to the body frame.'
port_driven
end