Drone loses API Connection and directly falls to ground

[Siddharth2511]

Code:

from sre_constants import IN
from colorama import init
import cosysairsim as airsim
import os
import cv2
import numpy as np 
import pprint
import time

from traitlets import ValidateHandler

# code to detect drone and then follow it

# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()

veh_P_name = "Drone1" # pursuer 
veh_E_name = "Drone2" # evader

# Enable API control for both vehicles
client.enableApiControl(True, veh_P_name)
client.enableApiControl(True, veh_E_name)

client.armDisarm(True, vehicle_name = veh_P_name)
client.armDisarm(True, vehicle_name = veh_E_name)

# TAKE OFF
client.takeoffAsync(vehicle_name = veh_P_name).join()
client.takeoffAsync(vehicle_name = veh_E_name).join()

# GTU Coordinates TAKE OFF
d1 = client.simGetGroundTruthKinematics(vehicle_name = veh_P_name)
print(f"Take off Pursuer: {d1}")

d2 = client.simGetGroundTruthKinematics(vehicle_name = veh_E_name)
print(f"Take off Evader: {d2}")

init_ht_P = -5
init_position_P = (0, 0, -2)

init_ht_E = -7
init_position_E = (2.5, 5, -2)

#DRONE MOVES TO INIT POS

client.enableApiControl(True, veh_P_name)
client.confirmConnection()
client.moveToPositionAsync(init_position_P[0], init_position_P[1], init_ht_P, 1, vehicle_name = veh_P_name).join()

d1 = client.simGetGroundTruthKinematics(vehicle_name = veh_P_name)
print(f"Pursuer init position: {d1}")

client.enableApiControl(True, veh_E_name)
client.confirmConnection()
client.moveToPositionAsync(init_position_E[0], init_position_E[1], init_ht_E, 1, vehicle_name = veh_E_name).join()

client.enableApiControl(True, veh_E_name)
client.confirmConnection()
client.hoverAsync(vehicle_name=veh_E_name).join()

d2 = client.simGetGroundTruthKinematics(vehicle_name = veh_E_name)
print(f"Evader init position: {d2}")

# set camera name and image type to request images and detections
camera_name = "0"
image_type = airsim.ImageType.Scene

# set detection radius in [cm]
client.simSetDetectionFilterRadius(camera_name, image_type, 200 * 100) 
# add desired object name to detect in wild card/regex format
client.simAddDetectionFilterMeshName(camera_name, image_type, "Drone2") 

client.enableApiControl(True, veh_E_name)
client.enableApiControl(True, veh_P_name)

while True:
    for i in range(20):
        print(f"{i} - ITERATION")
        d2 = client.simGetGroundTruthKinematics(vehicle_name = veh_E_name)
        print(f"Current Position Evader Drone: {d2}")

        rawImage = client.simGetImage(camera_name, image_type)

        if not rawImage:
            continue
        png = cv2.imdecode(airsim.string_to_uint8_array(rawImage), cv2.IMREAD_UNCHANGED)

        drone_evaders = client.simGetDetections(camera_name, image_type)

        if drone_evaders:

            print(f"Drone Detected at {i}")
            # time.sleep(1)
            for drone_evader in drone_evaders:
                s = pprint.pformat(drone_evader)
                print("Drone2: %s" % s)

                evader_kinematics = client.simGetGroundTruthKinematics(vehicle_name = drone_evader.name) ## GTU Coordinates of drone detected
                evader_position = evader_kinematics.position

                print(f"Evader Drone Ground Truth Position: {evader_position.x_val}, {evader_position.y_val}, {evader_position.z_val}")

                # Moving pursuer drone to the detected object's (evader's) ground truth position
                client.moveToPositionAsync(evader_position.x_val, evader_position.y_val, evader_position.z_val, 3, vehicle_name=veh_P_name).join()

                # Get ground truth coordinates of the pursuer drone after moving
                pursuer_kinematics = client.simGetGroundTruthKinematics(vehicle_name=veh_P_name)  # Pursuer drone
                print(f"Current Coordinates of Pursuer Drone: {pursuer_kinematics.position}")

                cv2.rectangle(png,(int(drone_evader.box2D.min.x_val),int(drone_evader.box2D.min.y_val)),(int(drone_evader.box2D.max.x_val),int(drone_evader.box2D.max.y_val)),(255,0,0),2)
                cv2.putText(png, drone_evader.name, (int(drone_evader.box2D.min.x_val),int(drone_evader.box2D.min.y_val - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (36,255,12))

    cv2.imshow("AirSim", png)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
    elif cv2.waitKey(1) & 0xFF == ord('c'):
        client.simClearDetectionMeshNames(camera_name, image_type)
    elif cv2.waitKey(1) & 0xFF == ord('a'):
        client.simAddDetectionFilterMeshName(camera_name, image_type, "Drone2")
cv2.destroyAllWindows() 

Drone2 (Evader Drone) looses API Connection as soon as Drone1 (Pursuer Drone) starts moving. As soon as the code enters "for drone_evader in drone_evaders:" loop the drone falls.

I have tried placing hoverAsync().join() at various points in code but the function seems futile. Any help would be appreciated. Also, do I have to establish API Connection, every time I use moveToPositionAsync()

[Siddharth2511]

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/main/docs/settings.md",
  "SettingsVersion": 2,
  "SimMode": "Multirotor",
  "Port": 41451,

  "Vehicles": {
    "Drone1": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": 0,
      "Y": 0,
      "Z": -2,
      "Yaw": 90
    },
    "Drone2": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": 2.5,
      "Y": 5,
      "Z": -2,
      "Yaw": -90
    }
  }
}

settings.json file being used

[WouterJansen]

Well the biggest reason is that with this configuration the evader drone is not in the FOV of the camera of pursuer and as such no detections are made and the loop fails. If I change to a configuration where the drone is in the FOV your code works fine for me. For example with:

{
    "SeeDocsAt": "https://cosys-lab.github.io/settings/",
    "SettingsVersion": 2,
    "ClockSpeed": 1,
    "LocalHostIp": "127.0.0.1",
    "ApiServerPort": 41451,
    "RpcEnabled": true,
    "SimMode": "Multirotor",
    "Vehicles": {
        "Drone1": {
            "VehicleType": "SimpleFlight",
            "AllowAPIAlways": true,
            "AllowAPIWhenDisconnected": true,
            "X": 0,
            "Y": 0,
            "Z": 0,
            "Yaw": 0
        },
        "Drone2": {
            "VehicleType": "SimpleFlight",
            "AllowAPIAlways": true,
            "AllowAPIWhenDisconnected": true,
            "X": 5,
            "Y": 0,
            "Z": 0,
            "Yaw": 0
        }
    }
}

The second problem I see with your code is that you have to remember that most functions such as simGetGroundTruthKinematics() and simGetVehiclePose() will return the coordinates in vehicle local frame, not in world frame. As such, when you move the pursuer drone to the local frame pose of the evader drone, it will not work as intended. See here for some more linked issues and discussions on this topic: https://github.com/microsoft/AirSim/pull/4282

You can use simGetObjectPose() which returns object poses in world frame, given the world frame is initially started from the starting point of the pursuer drone (the first defined vehicle in settings.json), this will match with the frame of the pursuer drone and will cause the pursuer to fly directly at the evader drone.

Some cleaning up of your code gives me a working example of the purseur flying towards the detected evader:

from sre_constants import IN
from colorama import init
import cosysairsim as airsim
import os
import cv2
import numpy as np
import pprint
import time

from traitlets import ValidateHandler

# code to detect drone and then follow it

# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.reset()

veh_P_name = "Drone1"  # pursuer
veh_E_name = "Drone2"  # evader

# Enable API control for both vehicles
client.enableApiControl(True, veh_P_name)
client.enableApiControl(True, veh_E_name)

client.armDisarm(True, vehicle_name=veh_P_name)
client.armDisarm(True, vehicle_name=veh_E_name)

# TAKE OFF
client.takeoffAsync(vehicle_name=veh_P_name).join()
client.takeoffAsync(vehicle_name=veh_E_name).join()

init_ht_P = -5
init_position_P = (0, 0, -2)

init_ht_E = -7
init_position_E = (2.5, 5, -2)

# DRONE MOVES TO INIT POS

client.moveToPositionAsync(init_position_P[0], init_position_P[1], init_ht_P, 1, vehicle_name=veh_P_name).join()
client.moveToPositionAsync(init_position_E[0], init_position_E[1], init_ht_E, 1, vehicle_name=veh_E_name).join()

# set camera name and image type to request images and detections
camera_name = "0"
image_type = airsim.ImageType.Scene

# set detection radius in [cm]
client.simSetDetectionFilterRadius(camera_name, image_type, 200 * 100)
# add desired object name to detect in wild card/regex format
client.simAddDetectionFilterMeshName(camera_name, image_type, "Drone2")

client.enableApiControl(True, veh_E_name)
client.enableApiControl(True, veh_P_name)

while True:
    for i in range(20):
        print(f"{i} - ITERATION")

        rawImage = client.simGetImage(camera_name, image_type)

        if not rawImage:
            continue
        png = cv2.imdecode(airsim.string_to_uint8_array(rawImage), cv2.IMREAD_UNCHANGED)

        drone_evaders = client.simGetDetections(camera_name, image_type)

        if drone_evaders:

            print(f"Drone Detected at {i}")
            # time.sleep(1)
            for drone_evader in drone_evaders:

                evader_pose = client.simGetObjectPose(drone_evader.name)
                evader_position = evader_pose.position

                print(f"Evader Drone Ground Truth Position: {evader_position.x_val}, {evader_position.y_val}, {evader_position.z_val}")

                # Moving pursuer drone to the detected object's (evader's) ground truth position
                client.moveToPositionAsync(evader_position.x_val, evader_position.y_val, evader_position.z_val, 3,
                                           vehicle_name=veh_P_name).join()

                cv2.rectangle(png, (int(drone_evader.box2D.min.x_val), int(drone_evader.box2D.min.y_val)),
                              (int(drone_evader.box2D.max.x_val), int(drone_evader.box2D.max.y_val)), (255, 0, 0), 2)
                cv2.putText(png, drone_evader.name,
                            (int(drone_evader.box2D.min.x_val), int(drone_evader.box2D.min.y_val - 10)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (36, 255, 12))

    cv2.imshow("AirSim", png)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
    elif cv2.waitKey(1) & 0xFF == ord('c'):
        client.simClearDetectionMeshNames(camera_name, image_type)
    elif cv2.waitKey(1) & 0xFF == ord('a'):
        client.simAddDetectionFilterMeshName(camera_name, image_type, "Drone2")
cv2.destroyAllWindows()