From a0923d6d3fc50343a0ebb190aecd764a6f14a87f Mon Sep 17 00:00:00 2001 From: nathanlct Date: Wed, 16 Nov 2022 13:05:42 -0600 Subject: [PATCH 1/3] smooth chatter --- onnx2ros/include/NetworkReader.h | 3 +++ onnx2ros/src/NetworkReader.cpp | 14 +++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/onnx2ros/include/NetworkReader.h b/onnx2ros/include/NetworkReader.h index 5fd4009..8acb648 100644 --- a/onnx2ros/include/NetworkReader.h +++ b/onnx2ros/include/NetworkReader.h @@ -17,6 +17,7 @@ #include #include #include +#include #include class BaseReader{ @@ -38,6 +39,8 @@ class BaseReader{ FILE* unit_test_file; FILE* unit_test_file_kathy; + int smooth_chatter_time_counter; + public: BaseReader(ros::NodeHandle *nh, std::string onnx_model_nathan, std::string onnx_model_kathy); diff --git a/onnx2ros/src/NetworkReader.cpp b/onnx2ros/src/NetworkReader.cpp index 38165d6..e07d81f 100644 --- a/onnx2ros/src/NetworkReader.cpp +++ b/onnx2ros/src/NetworkReader.cpp @@ -34,6 +34,8 @@ BaseReader::BaseReader(ros::NodeHandle *nh, std::string onnx_model_nathan, std:: for (int i = 0; i < 6; i++) { prev_accels.push_back(0.0); } + + smooth_chatter_time_counter = 0; // number of time steps since the speed setting was actuated } std::vector BaseReader::forward(std::vector input_values) { @@ -246,7 +248,17 @@ void PromptReader::publish() { temp = clamp(temp, lower_bound, upper_bound); temp = clamp(static_cast(temp), 20, 73); - msg_speed.data = temp; + // dont change the speed setting, unless requested speed setting is more than + // 1 mph away from current speed setting, or unless it has been a certain time + // since we last authorized a change + if (std::abs(state_setspeed.data - temp) > 1.5 || smooth_chatter_time_counter > 30) { + msg_speed.data = temp; + smooth_chatter_time_counter = 0; + } else { + msg_speed.data = state_setspeed.data; + smooth_chatter_time_counter++; + } + msg_gap.data = PromptReader::convertGapDataToSetting(result[1]); // std::cout << "NN output: " << msg_speed.data << " , avg_speed: " << avg_speed << " ,clamped val: " << clamped_val << "\n"; // std::cout << "Speed planner speed: " << state_spspeed.data << "\n"; From 4dc247723c22bbef6097ea8cd44bbc9abbb2f24f Mon Sep 17 00:00:00 2001 From: nathanlct Date: Wed, 16 Nov 2022 13:20:38 -0600 Subject: [PATCH 2/3] dont use std::abs --- onnx2ros/include/NetworkReader.h | 1 - onnx2ros/src/NetworkReader.cpp | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/onnx2ros/include/NetworkReader.h b/onnx2ros/include/NetworkReader.h index 8acb648..e096cad 100644 --- a/onnx2ros/include/NetworkReader.h +++ b/onnx2ros/include/NetworkReader.h @@ -17,7 +17,6 @@ #include #include #include -#include #include class BaseReader{ diff --git a/onnx2ros/src/NetworkReader.cpp b/onnx2ros/src/NetworkReader.cpp index e07d81f..c7f5a68 100644 --- a/onnx2ros/src/NetworkReader.cpp +++ b/onnx2ros/src/NetworkReader.cpp @@ -251,7 +251,8 @@ void PromptReader::publish() { // dont change the speed setting, unless requested speed setting is more than // 1 mph away from current speed setting, or unless it has been a certain time // since we last authorized a change - if (std::abs(state_setspeed.data - temp) > 1.5 || smooth_chatter_time_counter > 30) { + if ((state_setspeed.data - temp > 1.5) || (state_setspeed.data - temp < -1.5) + || (smooth_chatter_time_counter > 30)) { msg_speed.data = temp; smooth_chatter_time_counter = 0; } else { From aa8556a64dad97c3585763c28cab9c4df8eb9b76 Mon Sep 17 00:00:00 2001 From: nathanlct Date: Wed, 16 Nov 2022 14:26:40 -0600 Subject: [PATCH 3/3] Try different fix --- onnx2ros/include/NetworkReader.h | 2 +- onnx2ros/src/NetworkReader.cpp | 46 +++++++++++++++++++++++++------- 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/onnx2ros/include/NetworkReader.h b/onnx2ros/include/NetworkReader.h index e096cad..1a40112 100644 --- a/onnx2ros/include/NetworkReader.h +++ b/onnx2ros/include/NetworkReader.h @@ -38,7 +38,7 @@ class BaseReader{ FILE* unit_test_file; FILE* unit_test_file_kathy; - int smooth_chatter_time_counter; + std::vector last_requested_speeds; public: BaseReader(ros::NodeHandle *nh, std::string onnx_model_nathan, std::string onnx_model_kathy); diff --git a/onnx2ros/src/NetworkReader.cpp b/onnx2ros/src/NetworkReader.cpp index c7f5a68..4dfeef1 100644 --- a/onnx2ros/src/NetworkReader.cpp +++ b/onnx2ros/src/NetworkReader.cpp @@ -35,6 +35,8 @@ BaseReader::BaseReader(ros::NodeHandle *nh, std::string onnx_model_nathan, std:: prev_accels.push_back(0.0); } + last_requested_speeds = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; + smooth_chatter_time_counter = 0; // number of time steps since the speed setting was actuated } @@ -248,18 +250,42 @@ void PromptReader::publish() { temp = clamp(temp, lower_bound, upper_bound); temp = clamp(static_cast(temp), 20, 73); - // dont change the speed setting, unless requested speed setting is more than - // 1 mph away from current speed setting, or unless it has been a certain time - // since we last authorized a change - if ((state_setspeed.data - temp > 1.5) || (state_setspeed.data - temp < -1.5) - || (smooth_chatter_time_counter > 30)) { - msg_speed.data = temp; - smooth_chatter_time_counter = 0; - } else { - msg_speed.data = state_setspeed.data; - smooth_chatter_time_counter++; + // fix chatter: if oscillating in n and n+1 requests for the last second, stop switching + bool change_speed_setting = false; + + // find out if `last_requested_speeds` only contains two different values n and n+1 + int unique_val_1 = -99; + int unique_val_2 = -99; + for (int i = 0; i < 10; ++i) { + int val = last_requested_speeds[i]; + // if we already know that value, ignore + if (val == unique_val_1) continue; + else if (val == unique_val_2) continue; + // if not and we've seen 0 or 1 unique values so far, store it + else if (unique_val_1 == -99) unique_val_1 = val; + else if (unique_val_2 == -99) unique_val_2 = val; + // we already have 2 unique values and are seeing a third one + else { change_speed_setting = true; break; } + } + if (change_speed_setting == false) { + // we have less than 2 unique values + if (unique_val_1 == -99 || unique_val_2 == -99) + change_speed_setting = true; + // two unique values but not adjacent + else if (unique_val_1 - unique_val_2 != 1 && unique_val_1 - unique_val_2 != -1) + change_speed_setting = true; } + // send speed setting request + if (change_speed_setting) + msg_speed.data = temp; // use requested speed setting + else + msg_speed.data = state_setspeed.data; // keep current speed setting + + // append previous rotate to stack + last_requested_speeds.insert(last_requested_speeds.begin(), (int)temp); // mph + last_requested_speeds.pop_back(); + msg_gap.data = PromptReader::convertGapDataToSetting(result[1]); // std::cout << "NN output: " << msg_speed.data << " , avg_speed: " << avg_speed << " ,clamped val: " << clamped_val << "\n"; // std::cout << "Speed planner speed: " << state_spspeed.data << "\n";