Conflict between Offboard and Position mode when drone lands in gazebo simulation

[Nhi262]

Here my node using mavros API with some features:

• Set mode offboard via RC, then arm and takeoff automatically by ros2 node
• After 20 seconds, drone land and disarm the propeller.
  But my issues is:
  When the land_trigger is true, drone land on the ground but can not disarm and takeoff again.
  `#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy

class OffBoardControlNode(Node):
def init(self):
super().init("drone_offboard_control")
self.get_logger().info("Starting offboard control node...")
self.state = State()
self.flight_start_time = self.get_clock().now()
self.takeoff_height = 2.0
self.takeoff_triggered = False
self.landing_triggered = False

    self.qos_profile_pub = QoSProfile(
        reliability=QoSReliabilityPolicy.RELIABLE,
        durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
        depth=10)

    self.get_logger().info("[Reliability] : Reliable")
    self.get_logger().info("[Durability] : Transient Local")

    self.state_sub = self.create_subscription(State, '/mavros/state', self.state_cb, self.qos_profile_pub)
    self.local_pos_pub = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', self.qos_profile_pub)
    self.arming_client = self.create_client(CommandBool, '/mavros/cmd/arming', qos_profile=self.qos_profile_pub)
    self.land_client = self.create_client(CommandTOL, '/mavros/cmd/land', qos_profile=self.qos_profile_pub)
    self.set_mode_client = self.create_client(SetMode, '/mavros/set_mode', qos_profile=self.qos_profile_pub)

    self.timer = self.create_timer(0.1, self.publish_target_position)

def state_cb(self, msg):
    self.state = msg
    self.get_logger().info(f"Mode: {self.state.mode}, Armed: {self.state.armed}, Connected: {self.state.connected}")

    if self.state.mode == "OFFBOARD" and not self.landing_triggered:
        if not self.state.armed:
            self.arm_drone()  
        if self.state.armed and not self.takeoff_triggered:
            self.takeoff()  

    flight_duration = (self.get_clock().now() - self.flight_start_time).nanoseconds / 1e9
    if flight_duration >= 10 and not self.landing_triggered:
        self.get_logger().info("10 seconds of flight passed. Switching to RTL mode before landing...")
        self.set_mode_to_return_and_land()

def arm_drone(self):
    self.get_logger().info("Arming drone...")
    req = CommandBool.Request()
    req.value = True
    future = self.arming_client.call_async(req)
    future.add_done_callback(self.arm_callback)

def arm_callback(self, future):
    try:
        response = future.result()
        if response.success:
            self.get_logger().info("Drone Armed!")
        else:
            self.get_logger().warn("Failed to Arm Drone!")
    except Exception as e:
        self.get_logger().error(f"Service call failed: {str(e)}")

def takeoff(self):
    self.get_logger().info("Taking off...")
    self.takeoff_triggered = True
    target_pose = PoseStamped()
    target_pose.header.stamp = self.get_clock().now().to_msg()
    target_pose.pose.position.z = self.takeoff_height  
    self.local_pos_pub.publish(target_pose)

def publish_target_position(self):
    if self.landing_triggered:  
        return  

    target_pose = PoseStamped()
    target_pose.header.stamp = self.get_clock().now().to_msg()
    target_pose.pose.position.z = self.takeoff_height if self.takeoff_triggered else 0.0
    self.local_pos_pub.publish(target_pose)

def set_mode_to_return_and_land(self):
    self.get_logger().info("Switching to Return to Launch (RTL) mode...")
    req = SetMode.Request()
    req.custom_mode = "RTL"  
    future = self.set_mode_client.call_async(req)
    future.add_done_callback(self.mode_callback)

def mode_callback(self, future):
    try:
        response = future.result()
        if response.mode_sent:
            self.get_logger().info("Mode switched to RTL successfully!")
            
            # Mark landing as triggered and proceed
            self.landing_triggered = True
            self.land()
        else:
            self.get_logger().warn("Failed to switch to RTL mode!")
    except Exception as e:
        self.get_logger().error(f"Service call failed: {str(e)}")

def land(self):
    self.get_logger().info("Initiating landing...")
    req = CommandTOL.Request()
    future = self.land_client.call_async(req)
    future.add_done_callback(self.land_callback)

def land_callback(self, future):
    try:
        response = future.result()
        if response.success:
            self.get_logger().info("Landing successful!")
            
            # Switch to STABILIZED mode before disarming
            self.set_mode_to_stabilized()  
            self.disarm_drone()  
        else:
            self.get_logger().warn("Failed to initiate landing!")
    except Exception as e:
        self.get_logger().error(f"Service call failed: {str(e)}")

def set_mode_to_stabilized(self):
    self.get_logger().info("Switching to STABILIZED mode before disarming...")
    req = SetMode.Request()
    req.custom_mode = "STABILIZED"  
    future = self.set_mode_client.call_async(req)
    future.add_done_callback(self.mode_callback)

def disarm_drone(self):
    self.get_logger().info("Disarming drone after landing...")
    req = CommandBool.Request()
    req.value = False  
    future = self.arming_client.call_async(req)
    future.add_done_callback(self.arm_callback)

def main(args=None):
rclpy.init(args=args)
node = OffBoardControlNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()

if name == "main":
main()
`