feat(ad_service_state_monitor): limit odometry buffer size (#514)

* feat(ad_service_state_monitor): limit odometry buffer size 40 Signed-off-by: Takayuki Murooka <[email protected]> * Update system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp Co-authored-by: Kenji Miyake <[email protected]> * Update ad_service_state_monitor_node.cpp Co-authored-by: Kenji Miyake <[email protected]>
tier4 · May 19, 2022 · e9dafd9 · e9dafd9
1 parent 7d2ac35
commit e9dafd9
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/...service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/...service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp
@@ -120,6 +120,11 @@ void AutowareStateMonitorNode::onOdometry(const nav_msgs::msg::Odometry::ConstSh
 
     state_input_.odometry_buffer.pop_front();
   }
+
+  constexpr size_t odometry_buffer_size = 200;  // 40Hz * 5sec
+  if (state_input_.odometry_buffer.size() > odometry_buffer_size) {
+    state_input_.odometry_buffer.pop_front();
+  }
 }
 
 bool AutowareStateMonitorNode::onShutdownService(