fix(static_storage): Throw if an invalid entry was detected

Signed-off-by: Janosch Machowinski <[email protected]>

Author: Janosch Machowinski

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -160,6 +160,14 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom
       services_,
       waitables_
     );
+
+    if(this->needs_pruning_) {
+      // we need to throw here, as the indexing of the rcl_waitset is broken,
+      // in case of invalid entries
+
+      throw std::runtime_error(
+            "StaticStorage : storage_rebuild_rcl_wait_set: Detected invalid entity in static entity storage");
+    }
   }
 
   // storage_add_subscription() explicitly not declared here