diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp
index 7e924d7de5..6fe052bec0 100644
--- a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp
+++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 #pragma once
 
+#include <algorithm>
 #include <deque>
 #include <functional>
 #include <list>
@@ -214,8 +215,12 @@ class CBGScheduler
   void remove_callback_group(const CallbackGroupHandle *callback_handle)
   {
     std::lock_guard lk(ready_callback_groups_mutex);
-    ready_callback_groups.erase(std::find(ready_callback_groups.begin(),
-          ready_callback_groups.end(), callback_handle));
+
+    auto cbg_it = std::find(ready_callback_groups.begin(),
+          ready_callback_groups.end(), callback_handle);
+    if (cbg_it != ready_callback_groups.end()) {
+      ready_callback_groups.erase(cbg_it);
+    }
 
     callback_groups.remove_if([&callback_handle] (const auto & e) {
         return e.get() == callback_handle;
@@ -230,11 +235,13 @@ class CBGScheduler
    */
   void callback_group_ready(CallbackGroupHandle *handle, bool callback_group_was_idle)
   {
-    if (!handle->in_queue) {
+    {
       std::lock_guard l(ready_callback_groups_mutex);
 
-      ready_callback_groups.push_back(handle);
-      handle->in_queue = true;
+      if (!handle->in_queue) {
+        ready_callback_groups.push_back(handle);
+        handle->in_queue = true;
+      }
     }
 
     if(callback_group_was_idle) {