Skip to content

Commit c1b2281

Browse files
Ishan1923Ishan1923
committed
Fix: Prevent invalid odometry updates on NaN input
- Added isfinite check in update_and_write_commands to protect last_linear_velocity_ - Added regression test confirming robot stops safely on NaN input Signed-off-by: Ishan1923 <[email protected]>
1 parent 8549447 commit c1b2281

File tree

7 files changed

+81
-77
lines changed

7 files changed

+81
-77
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp‎

Lines changed: 5 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -59,9 +59,7 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
5959

6060
if (params_.open_loop)
6161
{
62-
if(!odometry_.try_update_open_loop(
63-
last_linear_velocity_, last_angular_velocity_, period.seconds()
64-
))return false;
62+
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
6563
}
6664
else
6765
{
@@ -91,31 +89,23 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
9189
const double steering_left_position = steering_left_position_op.value();
9290

9391
if (
94-
!std::isfinite(traction_right_wheel_value) || !std::isfinite(traction_left_wheel_value) ||
95-
!std::isfinite(steering_right_position) || !std::isfinite(steering_left_position))
92+
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
93+
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
9694
{
97-
RCLCPP_DEBUG(logger, "Odometery failed! : sensors returning infinity or NaN!");
98-
return false;
99-
}
100-
else
101-
{
102-
bool success = false;
10395
if (params_.position_feedback)
10496
{
10597
// Estimate linear and angular velocity using joint information
106-
success = odometry_.update_from_position(
98+
odometry_.update_from_position(
10799
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
108100
steering_left_position, period.seconds());
109101
}
110102
else
111103
{
112104
// Estimate linear and angular velocity using joint information
113-
success = odometry_.update_from_velocity(
105+
odometry_.update_from_velocity(
114106
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
115107
steering_left_position, period.seconds());
116108
}
117-
118-
return success;
119109
}
120110
}
121111
return true;

‎steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -139,8 +139,6 @@ class SteeringKinematics
139139
*/
140140
void update_open_loop(const double v_bx, const double omega_bz, const double dt);
141141

142-
bool try_update_open_loop(double linear, double angular, double delTime);
143-
144142
/**
145143
* \brief Set odometry type
146144
* \param type odometry type

‎steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -116,8 +116,6 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee
116116

117117
void reset_odometry();
118118

119-
bool try_update_open_loop(double linear, double angular, double delTime);
120-
121119
private:
122120
steering_kinematics::SteeringKinematics sk_impl_;
123121
};

‎steering_controllers_library/src/steering_controllers_library.cpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -520,8 +520,14 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
520520
auto logger = get_node()->get_logger();
521521

522522
// store current ref (for open loop odometry) and update odometry
523-
last_linear_velocity_ = reference_interfaces_[0];
524-
last_angular_velocity_ = reference_interfaces_[1];
523+
if (std::isfinite(reference_interfaces_[0]))
524+
{
525+
last_linear_velocity_ = reference_interfaces_[0];
526+
}
527+
if (std::isfinite(reference_interfaces_[1]))
528+
{
529+
last_angular_velocity_ = reference_interfaces_[1];
530+
}
525531
update_odometry(period);
526532

527533
// MOVE ROBOT

‎steering_controllers_library/src/steering_kinematics.cpp‎

Lines changed: 1 addition & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -78,9 +78,6 @@ bool SteeringKinematics::update_odometry(
7878
bool SteeringKinematics::update_from_position(
7979
const double traction_wheel_pos, const double steer_pos, const double dt)
8080
{
81-
82-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
83-
8481
const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_;
8582

8683
/// Update old position with current:
@@ -93,11 +90,6 @@ bool SteeringKinematics::update_from_position(
9390
const double traction_right_wheel_pos, const double traction_left_wheel_pos,
9491
const double steer_pos, const double dt)
9592
{
96-
97-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos)
98-
|| !std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos))
99-
return false;
100-
10193
const double traction_right_wheel_est_pos_diff =
10294
traction_right_wheel_pos - traction_right_wheel_old_pos_;
10395
const double traction_left_wheel_est_pos_diff =
@@ -115,11 +107,6 @@ bool SteeringKinematics::update_from_position(
115107
const double traction_right_wheel_pos, const double traction_left_wheel_pos,
116108
const double right_steer_pos, const double left_steer_pos, const double dt)
117109
{
118-
119-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos)
120-
|| !std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos))
121-
return false;
122-
123110
const double traction_right_wheel_est_pos_diff =
124111
traction_right_wheel_pos - traction_right_wheel_old_pos_;
125112
const double traction_left_wheel_est_pos_diff =
@@ -137,9 +124,6 @@ bool SteeringKinematics::update_from_position(
137124
bool SteeringKinematics::update_from_velocity(
138125
const double traction_wheel_vel, const double steer_pos, const double dt)
139126
{
140-
141-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
142-
143127
steer_pos_ = steer_pos;
144128
double linear_velocity = traction_wheel_vel * wheel_radius_;
145129
const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_;
@@ -172,9 +156,6 @@ bool SteeringKinematics::update_from_velocity(
172156
const double right_traction_wheel_vel, const double left_traction_wheel_vel,
173157
const double steer_pos, const double dt)
174158
{
175-
176-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) || !std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
177-
178159
steer_pos_ = steer_pos;
179160
double linear_velocity = get_linear_velocity_double_traction_axle(
180161
right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
@@ -188,11 +169,6 @@ bool SteeringKinematics::update_from_velocity(
188169
const double right_traction_wheel_vel, const double left_traction_wheel_vel,
189170
const double right_steer_pos, const double left_steer_pos, const double dt)
190171
{
191-
192-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
193-
!std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
194-
195-
196172
// overdetermined, we take the average
197173
const double right_steer_pos_est = std::atan(
198174
wheel_base_ * std::tan(right_steer_pos) /
@@ -211,29 +187,14 @@ bool SteeringKinematics::update_from_velocity(
211187

212188
void SteeringKinematics::update_open_loop(const double v_bx, const double omega_bz, const double dt)
213189
{
214-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
215190
/// Save last linear and angular velocity:
216191
linear_ = v_bx;
217-
218192
angular_ = omega_bz;
219193

220194
/// Integrate odometry:
221195
integrate_fk(v_bx, omega_bz, dt);
222196
}
223197

224-
bool SteeringKinematics::try_update_open_loop(const double v_bx, const double omega_bz, const double dt)
225-
{
226-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
227-
/// Save last linear and angular velocity:
228-
linear_ = v_bx;
229-
230-
angular_ = omega_bz;
231-
232-
/// Integrate odometry:
233-
integrate_fk(v_bx, omega_bz, dt);
234-
return true;
235-
}
236-
237198
void SteeringKinematics::set_wheel_params(
238199
double wheel_radius, double wheel_base, double wheel_track)
239200
{
@@ -432,4 +393,5 @@ void SteeringKinematics::reset_accumulators()
432393
linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
433394
angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
434395
}
396+
435397
} // namespace steering_kinematics

‎steering_controllers_library/src/steering_odometry.cpp‎

Lines changed: 1 addition & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -33,16 +33,13 @@ void SteeringOdometry::init(const rclcpp::Time & time) { sk_impl_.init(time); }
3333
bool SteeringOdometry::update_from_position(
3434
const double traction_wheel_pos, const double steer_pos, const double dt)
3535
{
36-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
3736
return sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt);
3837
}
3938

4039
bool SteeringOdometry::update_from_position(
4140
const double traction_right_wheel_pos, const double traction_left_wheel_pos,
4241
const double steer_pos, const double dt)
4342
{
44-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
45-
!std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos)) return false;
4643
return sk_impl_.update_from_position(
4744
traction_right_wheel_pos, traction_left_wheel_pos, steer_pos, dt);
4845
}
@@ -51,25 +48,20 @@ bool SteeringOdometry::update_from_position(
5148
const double traction_right_wheel_pos, const double traction_left_wheel_pos,
5249
const double right_steer_pos, const double left_steer_pos, const double dt)
5350
{
54-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
55-
!std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
5651
return sk_impl_.update_from_position(
5752
traction_right_wheel_pos, traction_left_wheel_pos, right_steer_pos, left_steer_pos, dt);
5853
}
5954

6055
bool SteeringOdometry::update_from_velocity(
6156
const double traction_wheel_vel, const double steer_pos, const double dt)
6257
{
63-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
6458
return sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt);
6559
}
6660

6761
bool SteeringOdometry::update_from_velocity(
6862
const double right_traction_wheel_vel, const double left_traction_wheel_vel,
6963
const double steer_pos, const double dt)
7064
{
71-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
72-
!std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
7365
return sk_impl_.update_from_velocity(
7466
right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt);
7567
}
@@ -78,24 +70,15 @@ bool SteeringOdometry::update_from_velocity(
7870
const double right_traction_wheel_vel, const double left_traction_wheel_vel,
7971
const double right_steer_pos, const double left_steer_pos, const double dt)
8072
{
81-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
82-
!std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
8373
return sk_impl_.update_from_velocity(
8474
right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt);
8575
}
8676

8777
void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
8878
{
89-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
9079
sk_impl_.update_open_loop(v_bx, omega_bz, dt);
9180
}
9281

93-
bool SteeringOdometry::try_update_open_loop(const double v_bx, const double omega_bz, const double dt){
94-
if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
95-
sk_impl_.try_update_open_loop(v_bx, omega_bz, dt);
96-
return true;
97-
}
98-
9982
void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track)
10083
{
10184
sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track);
@@ -126,4 +109,5 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
126109
}
127110

128111
void SteeringOdometry::reset_odometry() { sk_impl_.reset_odometry(); }
112+
129113
} // namespace steering_odometry

‎steering_controllers_library/test/test_steering_controllers_library.cpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -386,6 +386,72 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout)
386386
EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6);
387387
}
388388

389+
TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
390+
{
391+
// 1. Setup Options
392+
auto node_options = controller_->define_custom_node_options();
393+
node_options.append_parameter_override("open_loop", true);
394+
node_options.append_parameter_override(
395+
"traction_joints_names", std::vector<std::string>{"wheel_left", "wheel_right"});
396+
node_options.append_parameter_override(
397+
"steering_joints_names", std::vector<std::string>{"steer_left", "steer_right"});
398+
SetUpController("test_steering_controllers_library", node_options);
399+
400+
// 2. Lifecycle
401+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
402+
controller_->set_chained_mode(false);
403+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
404+
405+
// 3. The Publicist Hack (Unlock Input AND Output)
406+
struct Publicist : public TestableSteeringControllersLibrary
407+
{
408+
// Unlock input buffer
409+
using steering_controllers_library::SteeringControllersLibrary::input_ref_;
410+
// Unlock the wheels (Output)
411+
using controller_interface::ControllerInterfaceBase::command_interfaces_;
412+
};
413+
auto * pub_controller = static_cast<Publicist *>(controller_.get());
414+
415+
// 4. Send Valid Command (1.5 m/s)
416+
auto command_msg = ControllerReferenceMsg();
417+
command_msg.header.stamp = controller_->get_node()->now();
418+
command_msg.twist.linear.x = 1.5;
419+
command_msg.twist.angular.z = 0.0;
420+
421+
pub_controller->input_ref_.set(command_msg);
422+
423+
// 5. Force Update
424+
controller_->update_reference_from_subscribers(
425+
controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
426+
controller_->update_and_write_commands(
427+
controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
428+
429+
// 6. CHECK THE WHEELS
430+
// FIX: Use .get_optional().value() (This is how ROS 2 reads values back)
431+
double wheel_speed_1 = pub_controller->command_interfaces_[0].get_optional().value();
432+
433+
// It should be moving
434+
ASSERT_GT(wheel_speed_1, 0.1);
435+
436+
// 7. Send NaN Command
437+
auto nan_msg = ControllerReferenceMsg();
438+
nan_msg.header.stamp = controller_->get_node()->now();
439+
nan_msg.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
440+
nan_msg.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
441+
442+
pub_controller->input_ref_.set(nan_msg);
443+
444+
// 8. Force Update Again
445+
controller_->update_reference_from_subscribers(
446+
controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
447+
controller_->update_and_write_commands(
448+
controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
449+
450+
// 9. THE PROOF
451+
// The wheel speed should stay exactly the same
452+
EXPECT_DOUBLE_EQ(pub_controller->command_interfaces_[0].get_optional().value(), 0.0);
453+
}
454+
389455
int main(int argc, char ** argv)
390456
{
391457
::testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)