From b61fb3d851ba72a1afe3c6863f75f49e4f7a1e87 Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Mon, 10 May 2021 15:13:58 +0200 Subject: [PATCH 1/4] guard dt negative case and break the controller --- .../src/ackermann_steering_controller.cpp | 6 ++++++ diff_drive_controller/src/diff_drive_controller.cpp | 6 ++++++ .../src/four_wheel_steering_controller.cpp | 10 ++++++++++ 3 files changed, 22 insertions(+) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 663b8662e..0a7386f77 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -322,6 +322,12 @@ namespace ackermann_steering_controller{ // Limit velocities and accelerations: const double cmd_dt(period.toSec()); + if(cmd_dt < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + curr_cmd.lin = 0.0; + curr_cmd.ang = 0.0; + } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 018bd177c..1de733234 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -471,6 +471,12 @@ namespace diff_drive_controller{ // Limit velocities and accelerations: const double cmd_dt(period.toSec()); + if(cmd_dt < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + curr_cmd.lin = 0.0; + curr_cmd.ang = 0.0; + } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 0c5dbd91e..8cf6064dc 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -363,6 +363,16 @@ namespace four_wheel_steering_controller{ } const double cmd_dt(period.toSec()); + if(cmd_dt < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + curr_cmd_twist.lin_x = 0.0; + curr_cmd_twist.lin_y = 0.0; + curr_cmd_twist.ang = 0.0; + curr_cmd_4ws.lin = 0.0; + curr_cmd_4ws.front_steering = 0.0; + curr_cmd_4ws.rear_steering = 0.0; + } const double angular_speed = odometry_.getAngular(); const double steering_track = track_-2*wheel_steering_y_offset_; From b69548ff4bcbc52845231df40b371986b1852c53 Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Tue, 25 May 2021 17:00:22 +0200 Subject: [PATCH 2/4] set cmd_dt to zero in case period is negative --- .../src/ackermann_steering_controller.cpp | 2 +- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- .../src/four_wheel_steering_controller.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 0a7386f77..f24291bd7 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -321,7 +321,7 @@ namespace ackermann_steering_controller{ } // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1de733234..ab0b453f6 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -470,7 +470,7 @@ namespace diff_drive_controller{ } // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 8cf6064dc..69528250a 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -362,7 +362,7 @@ namespace four_wheel_steering_controller{ curr_cmd_4ws.rear_steering = 0.0; } - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); From 1e8bf64b23e1c8d2c49f6650014cff103e5abc0e Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Thu, 10 Jun 2021 12:52:12 +0200 Subject: [PATCH 3/4] remove setting curr_cmd to zero when cmd_dt is negative to avoid abrupt changes in control --- .../src/ackermann_steering_controller.cpp | 2 -- diff_drive_controller/src/diff_drive_controller.cpp | 2 -- .../src/four_wheel_steering_controller.cpp | 6 ------ 3 files changed, 10 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f24291bd7..b647a8fbf 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -325,8 +325,6 @@ namespace ackermann_steering_controller{ if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); - curr_cmd.lin = 0.0; - curr_cmd.ang = 0.0; } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ab0b453f6..3f7536fb9 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -474,8 +474,6 @@ namespace diff_drive_controller{ if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); - curr_cmd.lin = 0.0; - curr_cmd.ang = 0.0; } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 69528250a..0ef4de633 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -366,12 +366,6 @@ namespace four_wheel_steering_controller{ if(cmd_dt < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); - curr_cmd_twist.lin_x = 0.0; - curr_cmd_twist.lin_y = 0.0; - curr_cmd_twist.ang = 0.0; - curr_cmd_4ws.lin = 0.0; - curr_cmd_4ws.front_steering = 0.0; - curr_cmd_4ws.rear_steering = 0.0; } const double angular_speed = odometry_.getAngular(); From db98eae40d1724e6046a5e9284543ff6eff6373f Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Thu, 23 Sep 2021 13:35:07 +0200 Subject: [PATCH 4/4] correct negative time check for error log Since we are already setting cmd_dt to zero in the case of negative period, we need to check the period itself in order to log the error --- .../src/ackermann_steering_controller.cpp | 2 +- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- .../src/four_wheel_steering_controller.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index b647a8fbf..003efc08e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -322,7 +322,7 @@ namespace ackermann_steering_controller{ // Limit velocities and accelerations: const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; - if(cmd_dt < 0) + if(period.toSec() < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 3f7536fb9..dc5892503 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -471,7 +471,7 @@ namespace diff_drive_controller{ // Limit velocities and accelerations: const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; - if(cmd_dt < 0) + if(period.toSec() > 0 < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); } diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 0ef4de633..b5b27e9b5 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -363,7 +363,7 @@ namespace four_wheel_steering_controller{ } const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; - if(cmd_dt < 0) + if(period.toSec() < 0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); }