Skip to content

Commit 48de1a4

Browse files
committed
Return update_odometry() instead of return true
1 parent a3083e8 commit 48de1a4

File tree

1 file changed

+10
-20
lines changed

1 file changed

+10
-20
lines changed

steering_controllers_library/src/steering_odometry.cpp

Lines changed: 10 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -87,9 +87,7 @@ bool SteeringOdometry::update_from_position(
8787
steer_pos_ = steer_pos;
8888
const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
8989

90-
update_odometry(linear_velocity, angular, dt);
91-
92-
return true;
90+
return update_odometry(linear_velocity, angular, dt);
9391
}
9492

9593
bool SteeringOdometry::update_from_position(
@@ -113,9 +111,8 @@ bool SteeringOdometry::update_from_position(
113111
(traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
114112
steer_pos_ = steer_pos;
115113
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);
119116
}
120117

121118
bool SteeringOdometry::update_from_position(
@@ -141,9 +138,7 @@ bool SteeringOdometry::update_from_position(
141138
steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
142139
const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
143140

144-
update_odometry(linear_velocity, angular, dt);
145-
146-
return true;
141+
return update_odometry(linear_velocity, angular, dt);
147142
}
148143

149144
bool SteeringOdometry::update_from_velocity(
@@ -153,9 +148,7 @@ bool SteeringOdometry::update_from_velocity(
153148
double linear_velocity = traction_wheel_vel * wheel_radius_;
154149
const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
155150

156-
update_odometry(linear_velocity, angular, dt);
157-
158-
return true;
151+
return update_odometry(linear_velocity, angular, dt);
159152
}
160153

161154
bool SteeringOdometry::update_from_velocity(
@@ -167,9 +160,8 @@ bool SteeringOdometry::update_from_velocity(
167160
steer_pos_ = steer_pos;
168161

169162
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);
173165
}
174166

175167
bool SteeringOdometry::update_from_velocity(
@@ -181,9 +173,7 @@ bool SteeringOdometry::update_from_velocity(
181173
(right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
182174
const double angular = steer_pos_ * linear_velocity / wheelbase_;
183175

184-
update_odometry(linear_velocity, angular, dt);
185-
186-
return true;
176+
return update_odometry(linear_velocity, angular, dt);
187177
}
188178

189179
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)
320310
void SteeringOdometry::integrate_exact(double linear, double angular)
321311
{
322312
if (fabs(angular) < 1e-6)
323-
{
313+
{
324314
integrate_runge_kutta_2(linear, angular);
325-
}
315+
}
326316
else
327317
{
328318
/// Exact integration (should solve problems when angular is zero):

0 commit comments

Comments
 (0)