From d9677801887f4d149d6d5044503c4b0a3fa8a096 Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Mon, 10 May 2021 15:13:58 +0200 Subject: [PATCH] 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 | 6 ++++++ 3 files changed, 18 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 60bf846fa..a841f61e9 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -470,6 +470,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 45cd7e7fb..76a625d18 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -362,6 +362,12 @@ 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.lin = 0.0; + curr_cmd.ang = 0.0; + } const double angular_speed = odometry_.getAngular(); const double steering_track = track_-2*wheel_steering_y_offset_;