@@ -87,9 +87,7 @@ bool SteeringOdometry::update_from_position(
87
87
steer_pos_ = steer_pos;
88
88
const double angular = tan (steer_pos) * linear_velocity / wheelbase_;
89
89
90
- update_odometry (linear_velocity, angular, dt);
91
-
92
- return true ;
90
+ return update_odometry (linear_velocity, angular, dt);
93
91
}
94
92
95
93
bool SteeringOdometry::update_from_position (
@@ -113,9 +111,8 @@ bool SteeringOdometry::update_from_position(
113
111
(traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
114
112
steer_pos_ = steer_pos;
115
113
const double angular = tan (steer_pos_) * linear_velocity / wheelbase_;
116
- update_odometry (linear_velocity, angular, dt);
117
-
118
- return true ;
114
+
115
+ return update_odometry (linear_velocity, angular, dt);
119
116
}
120
117
121
118
bool SteeringOdometry::update_from_position (
@@ -141,9 +138,7 @@ bool SteeringOdometry::update_from_position(
141
138
steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5 ;
142
139
const double angular = tan (steer_pos_) * linear_velocity / wheelbase_;
143
140
144
- update_odometry (linear_velocity, angular, dt);
145
-
146
- return true ;
141
+ return update_odometry (linear_velocity, angular, dt);
147
142
}
148
143
149
144
bool SteeringOdometry::update_from_velocity (
@@ -153,9 +148,7 @@ bool SteeringOdometry::update_from_velocity(
153
148
double linear_velocity = traction_wheel_vel * wheel_radius_;
154
149
const double angular = tan (steer_pos) * linear_velocity / wheelbase_;
155
150
156
- update_odometry (linear_velocity, angular, dt);
157
-
158
- return true ;
151
+ return update_odometry (linear_velocity, angular, dt);
159
152
}
160
153
161
154
bool SteeringOdometry::update_from_velocity (
@@ -167,9 +160,8 @@ bool SteeringOdometry::update_from_velocity(
167
160
steer_pos_ = steer_pos;
168
161
169
162
const double angular = tan (steer_pos_) * linear_velocity / wheelbase_;
170
- update_odometry (linear_velocity, angular, dt);
171
-
172
- return true ;
163
+
164
+ return update_odometry (linear_velocity, angular, dt);
173
165
}
174
166
175
167
bool SteeringOdometry::update_from_velocity (
@@ -181,9 +173,7 @@ bool SteeringOdometry::update_from_velocity(
181
173
(right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5 ;
182
174
const double angular = steer_pos_ * linear_velocity / wheelbase_;
183
175
184
- update_odometry (linear_velocity, angular, dt);
185
-
186
- return true ;
176
+ return update_odometry (linear_velocity, angular, dt);
187
177
}
188
178
189
179
void SteeringOdometry::update_open_loop (const double linear, const double angular, const double dt)
@@ -320,9 +310,9 @@ void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular)
320
310
void SteeringOdometry::integrate_exact (double linear, double angular)
321
311
{
322
312
if (fabs (angular) < 1e-6 )
323
- {
313
+ {
324
314
integrate_runge_kutta_2 (linear, angular);
325
- }
315
+ }
326
316
else
327
317
{
328
318
// / Exact integration (should solve problems when angular is zero):
0 commit comments