From b4d79ab1de030b2803682c1a1c058c14fd93b414 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 11 Jan 2025 15:52:07 +0100 Subject: [PATCH] Check dt in updateFromVelocity (backport #1481) (#1486) Co-authored-by: Tony Najjar --- diff_drive_controller/src/odometry.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 99bb22890d..95fa26b72b 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -74,7 +74,10 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); - + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } // Compute linear and angular diff: const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity