diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 7f5cad74ad..509a527ec6 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/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