wait for fmu before mode registration

Auterion · Jul 22, 2024 · bfebd82 · bfebd82
1 parent 411b3b1
commit bfebd82
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 3 deletions.
diff --git 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
@@ -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;
   }