Reorder method call in lifecycle to avoid node crashes + increase nr.…

… of retries for psdk init
umdlife · Jan 29, 2024 · 58b387b · 58b387b
1 parent 57b533b
commit 58b387b
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper_utils.hpp b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper_utils.hpp
@@ -41,7 +41,7 @@
 #define CONTROL_DATA_TOPICS_MAX_FREQ 50
 
 #define GOOD_GPS_SIGNAL_LEVEL 5
-#define MAX_NUMBER_OF_RETRIES 3
+#define MAX_NUMBER_OF_RETRIES 5
 
 namespace psdk_ros2
 {

diff --git a/psdk_wrapper/src/psdk_wrapper.cpp b/psdk_wrapper/src/psdk_wrapper.cpp
@@ -73,7 +73,6 @@ PSDKWrapper::on_configure(const rclcpp_lifecycle::State &state)
   {
     return CallbackReturn::FAILURE;
   }
-  initialize_ros_elements();
 
   return CallbackReturn::SUCCESS;
 }
@@ -89,17 +88,22 @@ PSDKWrapper::on_activate(const rclcpp_lifecycle::State &state)
 
   if (!init(&user_info))
   {
+    rclcpp::shutdown();
     return CallbackReturn::FAILURE;
   }
 
-  activate_ros_elements();
-
   if (!init_telemetry() || !init_flight_control() || !init_camera_manager() ||
       !init_gimbal_manager() || !init_liveview())
   {
+    rclcpp::shutdown();
     return CallbackReturn::FAILURE;
   }
 
+  // Initialize and activate ROS elements only after the DJI modules are
+  // initialized
+  initialize_ros_elements();
+  activate_ros_elements();
+
   if (params_.publish_transforms)
   {
     publish_static_transforms();