From 09a0d367f65c17ebf4a989686803664f8e96f383 Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Thu, 9 Jan 2025 16:42:57 -0800 Subject: [PATCH] avoid sending nan commands to hardware --- diff_drive_controller/src/diff_drive_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 6df51a8566..472ec43278 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -186,6 +186,12 @@ controller_interface::return_type DiffDriveController::update_and_write_commands double linear_command = reference_interfaces_[0]; double angular_command = reference_interfaces_[1]; + if(std::isnan(linear_command) || std::isnan(angular_command)) + { + // NaNs occur when the controller is first initialized and the reference interfaces are not yet set + return controller_interface::return_type::OK; + } + // Apply (possibly new) multipliers: const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;