check control authority before go to, add sleep after takeoff and land

Author: javilinos

src/as2_platform_dji_psdk.cpp

@@ -198,6 +198,7 @@ bool DJIMatricePSDKPlatform::ownSetPlatformControlMode(const as2_msgs::msg::Cont
       {
         // Obtain control authority
         RCLCPP_INFO(this->get_logger(), "HOVER || SPEED MODE: Obtain control authority");
+        success = ctl_authority_;
         break;
       }
     default:
@@ -276,6 +277,9 @@ bool DJIMatricePSDKPlatform::ownTakeoff()
   bool success = result && response->success;
   if (!success) {
     RCLCPP_INFO(this->get_logger(), "Could not takeoff due to '%s'", response->message.data());
+  } else {
+    // sleep 10 seconds to wait for the takeoff
+    rclcpp::sleep_for(std::chrono::seconds(10));
   }
   return success;
 }
@@ -290,6 +294,9 @@ bool DJIMatricePSDKPlatform::ownLand()
   bool success = result && response->success;
   if (!success) {
     RCLCPP_INFO(this->get_logger(), "Could not land due to '%s'", response->message.data());
+  } else {
+    // sleep 10 seconds to wait for the land
+    rclcpp::sleep_for(std::chrono::seconds(10));
   }
   return success;
 }