diff --git a/onnx2ros/include/NetworkReader.h b/onnx2ros/include/NetworkReader.h index 5fd4009..1a40112 100644 --- a/onnx2ros/include/NetworkReader.h +++ b/onnx2ros/include/NetworkReader.h @@ -38,6 +38,8 @@ class BaseReader{ FILE* unit_test_file; FILE* unit_test_file_kathy; + 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 38165d6..4dfeef1 100644 --- a/onnx2ros/src/NetworkReader.cpp +++ b/onnx2ros/src/NetworkReader.cpp @@ -34,6 +34,10 @@ BaseReader::BaseReader(ros::NodeHandle *nh, std::string onnx_model_nathan, std:: for (int i = 0; i < 6; i++) { 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 } std::vector BaseReader::forward(std::vector input_values) { @@ -246,7 +250,42 @@ void PromptReader::publish() { temp = clamp(temp, lower_bound, upper_bound); temp = clamp(static_cast(temp), 20, 73); - msg_speed.data = temp; + // 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";