feat: optimize control module safety management (#114)

Author: daohu527

modules/common/util/BUILD

@@ -65,6 +65,11 @@ cc_library(
     ]
 )
 
+cc_library(
+    name = "debouncer",
+    hdrs = ["debouncer.h"]
+)
+
 cc_library(
     name = "future",
     hdrs = ["future.h"],
@@ -292,4 +297,14 @@ cc_test(
     ],
 )
 
+cc_test(
+    name = "debouncer_test",
+    size = "small",
+    srcs = ["debouncer_test.cc"],
+    deps = [
+        ":debouncer",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
 cpplint()

modules/common/util/debouncer.h

@@ -0,0 +1,82 @@
+// Copyright 2025 WheelOS. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+//  Created Date: 2026-01-03
+//  Author: daohu527
+
+#pragma once
+
+#include <cstdint>
+
+namespace apollo {
+namespace control {
+
+/**
+ * @class CounterDebouncer
+ * @brief A counter-based signal debouncer.
+ * It confirms a fault only when the internal counter reaches a specific
+ * threshold.
+ */
+class CounterDebouncer {
+ public:
+  /**
+   * @brief Constructor
+   * @param threshold The number of consecutive fault detections required to
+   * confirm a fault.
+   */
+  explicit CounterDebouncer(uint32_t threshold)
+      : threshold_(threshold), count_(0) {}
+
+  /**
+   * @brief Update the debouncer state with a new sample.
+   * @param is_fault Current status of the monitored signal.
+   * @return True if the fault is confirmed (counter >= threshold).
+   */
+  bool Update(bool is_fault) {
+    if (is_fault) {
+      if (count_ < threshold_) {
+        count_++;
+      }
+    } else {
+      // Immediate reset strategy: resets the counter to zero as soon as a
+      // normal signal is received. This ensures high confidence for fault
+      // recovery.
+      count_ = 0;
+    }
+    return IsActive();
+  }
+
+  /**
+   * @brief Reset the internal counter to zero.
+   */
+  void Reset() { count_ = 0; }
+
+  /**
+   * @brief Check if the fault is currently active/confirmed.
+   * @return True if the counter has reached the threshold.
+   */
+  bool IsActive() const { return count_ >= threshold_ && threshold_ > 0; }
+
+  /**
+   * @brief Get the current counter value (useful for telemetry).
+   */
+  uint32_t count() const { return count_; }
+
+ private:
+  uint32_t threshold_;
+  uint32_t count_;
+};
+
+}  // namespace control
+}  // namespace apollo

modules/common/util/debouncer_test.cc

@@ -0,0 +1,151 @@
+// Copyright 2025 WheelOS. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+//  Created Date: 2026-01-03
+//  Author: daohu527
+
+#include "modules/common/util/debouncer.h"
+
+#include <gtest/gtest.h>
+
+namespace apollo {
+namespace control {
+
+/**
+ * @class CounterDebouncerTest
+ * @brief Test suite class for managing the CounterDebouncer test context.
+ */
+class CounterDebouncerTest : public ::testing::Test {
+ protected:
+  // Setup logic can be added here if needed
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+/**
+ * @test Verify if the initial state is correct after object construction.
+ */
+TEST_F(CounterDebouncerTest, Initialization) {
+  uint32_t threshold = 5;
+  CounterDebouncer debouncer(threshold);
+
+  EXPECT_EQ(debouncer.count(), 0u);
+  EXPECT_FALSE(debouncer.IsActive());
+}
+
+/**
+ * @test Verify the normal debouncing process until the threshold is reached.
+ */
+TEST_F(CounterDebouncerTest, NormalDebounceProcess) {
+  uint32_t threshold = 3;
+  CounterDebouncer debouncer(threshold);
+
+  // 1st fault sample: fault not yet confirmed
+  EXPECT_FALSE(debouncer.Update(true));
+  EXPECT_EQ(debouncer.count(), 1u);
+
+  // 2nd fault sample: fault not yet confirmed
+  EXPECT_FALSE(debouncer.Update(true));
+  EXPECT_EQ(debouncer.count(), 2u);
+
+  // 3rd fault sample: threshold reached, fault confirmed
+  EXPECT_TRUE(debouncer.Update(true));
+  EXPECT_EQ(debouncer.count(), 3u);
+  EXPECT_TRUE(debouncer.IsActive());
+
+  // 4th fault sample: counter should stay capped at threshold and remain active
+  EXPECT_TRUE(debouncer.Update(true));
+  EXPECT_EQ(debouncer.count(), 3u);
+}
+
+/**
+ * @test Verify the Immediate Reset Strategy.
+ * Even if the fault is close to being confirmed, a single normal signal
+ * must reset the counter to zero immediately.
+ */
+TEST_F(CounterDebouncerTest, ImmediateResetOnNormalSignal) {
+  CounterDebouncer debouncer(10);
+
+  // Simulate 9 consecutive fault samples
+  for (int i = 0; i < 9; ++i) {
+    debouncer.Update(true);
+  }
+  EXPECT_FALSE(debouncer.IsActive());
+  EXPECT_EQ(debouncer.count(), 9u);
+
+  // Receive one normal signal
+  EXPECT_FALSE(debouncer.Update(false));
+
+  // Counter must reset to zero immediately
+  EXPECT_EQ(debouncer.count(), 0u);
+  EXPECT_FALSE(debouncer.IsActive());
+}
+
+/**
+ * @test Verify the recovery logic after a fault has been confirmed.
+ */
+TEST_F(CounterDebouncerTest, RecoveryAfterConfirmation) {
+  CounterDebouncer debouncer(2);
+
+  // Confirm the fault
+  debouncer.Update(true);
+  debouncer.Update(true);
+  ASSERT_TRUE(debouncer.IsActive());
+
+  // Receive a normal signal; fault state should recover immediately
+  EXPECT_FALSE(debouncer.Update(false));
+  EXPECT_FALSE(debouncer.IsActive());
+  EXPECT_EQ(debouncer.count(), 0u);
+}
+
+/**
+ * @test Verify the manual Reset function.
+ */
+TEST_F(CounterDebouncerTest, ManualReset) {
+  CounterDebouncer debouncer(5);
+  debouncer.Update(true);
+  debouncer.Update(true);
+
+  debouncer.Reset();
+
+  EXPECT_EQ(debouncer.count(), 0u);
+  EXPECT_FALSE(debouncer.IsActive());
+}
+
+/**
+ * @test Boundary condition: threshold is 0.
+ * Based on the logic (threshold > 0), IsActive should always be false.
+ */
+TEST_F(CounterDebouncerTest, ZeroThresholdBoundary) {
+  CounterDebouncer debouncer(0);
+
+  EXPECT_FALSE(debouncer.IsActive());
+  EXPECT_FALSE(debouncer.Update(true));
+  EXPECT_FALSE(debouncer.IsActive());
+}
+
+/**
+ * @test Boundary condition: threshold is 1.
+ * Should trigger/confirm fault on the very first fault sample.
+ */
+TEST_F(CounterDebouncerTest, SingleSampleThreshold) {
+  CounterDebouncer debouncer(1);
+
+  EXPECT_TRUE(debouncer.Update(true));
+  EXPECT_TRUE(debouncer.IsActive());
+  EXPECT_FALSE(debouncer.Update(false));
+}
+
+}  // namespace control
+}  // namespace apollo

modules/control/BUILD

@@ -28,6 +28,7 @@ cc_library(
         "//modules/control/submodules:preprocessor_submodule_lib",
         "//modules/common_msgs/localization_msgs:localization_cc_proto",
         "//modules/common_msgs/planning_msgs:planning_cc_proto",
+        "//modules/control/safety:safety_manager",
         "@com_github_gflags_gflags//:gflags",
     ],
 )
@@ -65,7 +66,6 @@ install(
     data_dest = "control",
     data = [
         ":runtime_data",
-        "control.BUILD",
     ],
     targets = [
         ":libcontrol_component.so",
@@ -145,6 +145,7 @@ cc_test(
     data = ["//modules/control:test_data"],
     deps = [
         ":control_component_lib",
+        "//modules/common/util:util_lib",
         "@com_google_googletest//:gtest_main",
     ],
     linkstatic = True,