diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp
index 1ab8a07..72ab5a8 100644
--- a/px4_ros2_cpp/src/components/mode.cpp
+++ b/px4_ros2_cpp/src/components/mode.cpp
@@ -5,6 +5,7 @@
 
 #include "px4_ros2/components/mode.hpp"
 #include "px4_ros2/components/message_compatibility_check.hpp"
+#include "px4_ros2/components/wait_for_fmu.hpp"
 
 #include "registration.hpp"
 
@@ -55,8 +56,8 @@ bool ModeBase::doRegister()
 {
   assert(!_registration->registered());
 
-  if (!_skip_message_compatibility_check &&
-    !messageCompatibilityCheck(node(), {ALL_PX4_ROS2_MESSAGES}, topicNamespacePrefix()))
+  if (!_skip_message_compatibility_check && (!waitForFMU(node(), 15s) ||
+    !messageCompatibilityCheck(node(), {ALL_PX4_ROS2_MESSAGES}, topicNamespacePrefix())))
   {
     return false;
   }
diff --git a/px4_ros2_cpp/src/components/mode_executor.cpp b/px4_ros2_cpp/src/components/mode_executor.cpp
index a6cb297..9d2a573 100644
--- a/px4_ros2_cpp/src/components/mode_executor.cpp
+++ b/px4_ros2_cpp/src/components/mode_executor.cpp
@@ -5,6 +5,7 @@
 
 #include "px4_ros2/components/mode_executor.hpp"
 #include "px4_ros2/components/message_compatibility_check.hpp"
+#include "px4_ros2/components/wait_for_fmu.hpp"
 
 #include "registration.hpp"
 
@@ -50,7 +51,9 @@ bool ModeExecutorBase::doRegister()
 
   assert(!_registration->registered());
 
-  if (!messageCompatibilityCheck(_node, {ALL_PX4_ROS2_MESSAGES}, _topic_namespace_prefix)) {
+  if (!waitForFMU(node(), 15s) ||
+    !messageCompatibilityCheck(node(), {ALL_PX4_ROS2_MESSAGES}, _topic_namespace_prefix))
+  {
     return false;
   }