Drone starts moving parrallely and does not head towards given points in moveToPositionAsync()

[Siddharth2511]

I am working on a real time algorithm and my drones are not following the logic fed. Instead they start moving parallelly. I do not know much about API Connections, it says "API Connection lost, entering hover mode". Should I play around with time.sleep here?

Relevant Snippet:

far_P_index = 0
for i in range(100000):
    time.sleep(1)
    print(f"{i} - ITERATION")
    e =   client.simGetObjectPose(veh_E_name).position
    p =  [client.simGetObjectPose(veh_P1_name).position, client.simGetObjectPose(veh_P2_name).position, client.simGetObjectPose(veh_P3_name).position]

    print(f"Evader Drone Ground Truth Position: {e.x_val}, {e.y_val}, {e.z_val}")
    print(f"P1 Drone Ground Truth Position: {p[0].x_val}, {p[0].y_val}, {p[0].z_val}")
    print(f"P2 Drone Ground Truth Position: {p[1].x_val}, {p[1].y_val}, {p[1].z_val}")
    print(f"P3 Drone Ground Truth Position: {p[2].x_val}, {p[2].y_val}, {p[2].z_val}")

    rel_mag_wrt_e = [mag_3d(p[0]-e), mag_3d(p[1]-e), mag_3d(p[2]-e)]
    far_P_index = rel_mag_wrt_e.index(max(rel_mag_wrt_e)) + 1 

    print(f"Farthest: Drone{far_P_index}")
    # PURE PURSUIT FAR DRONE
    client.moveToPositionAsync(e.x_val, e.y_val, ht, 2,
                                vehicle_name=f"Drone{far_P_index}")

    x = circumcenter_2d((e.x_val, e.y_val), (p[(far_P_index)%3].x_val, p[(far_P_index)%3].y_val), (p[(far_P_index+1)%3].x_val, p[(far_P_index+1)%3].y_val))
    print(f"Circumcenter: {x, ht}")

    # MOVEMENT NEAR DRONES AND EVADER
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name=f"Drone{(far_P_index+1)%3}")
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name=f"Drone{(far_P_index+2)%3}")
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name="Drone0")

Entire 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
import math
import pandas as pd

from traitlets import ValidateHandler

# DEFINED FOR AIRSIM (USES .x_val and .y_val)
def mag_3d(vector):
    return math.sqrt(vector.x_val**2 + vector.y_val**2)


#CIRCUMCENTER TRIANGLE
def circumcenter_2d(A, B, C):
    """
    Calculates the circumcenter of a triangle in 2D given three points A, B, and C.
    
    Parameters:
    A, B, C (tuple): Tuples representing the coordinates of the three points in 2D (x, y).
    
    Returns:
    tuple: The coordinates of the circumcenter (x, y).
    """
    # Unpack the points
    x1, y1 = A
    x2, y2 = B
    x3, y3 = C
    
    # Calculate the midpoints of line segments AB and BC
    mid_AB = ((x1 + x2) / 2, (y1 + y2) / 2)
    mid_BC = ((x2 + x3) / 2, (y2 + y3) / 2)
    
    # Slopes of lines AB and BC
    slope_AB = (y2 - y1) / (x2 - x1) if x2 != x1 else None
    slope_BC = (y3 - y2) / (x3 - x2) if x3 != x2 else None
    
    # Perpendicular slopes
    perp_slope_AB = -1 / slope_AB if slope_AB is not None else 0  # Vertical slope means perpendicular is horizontal
    perp_slope_BC = -1 / slope_BC if slope_BC is not None else 0
    
    # Find the intersection of the two perpendicular bisectors
    # If the slopes are not vertical or horizontal, use point-slope form to find intersection
    if slope_AB is not None and slope_BC is not None:
        x_circumcenter = ((perp_slope_AB * mid_AB[0] - perp_slope_BC * mid_BC[0]) + (mid_BC[1] - mid_AB[1])) / (perp_slope_AB - perp_slope_BC)
        y_circumcenter = perp_slope_AB * (x_circumcenter - mid_AB[0]) + mid_AB[1]
    elif slope_AB is None:  # AB is vertical
        x_circumcenter = mid_AB[0]
        y_circumcenter = perp_slope_BC * (x_circumcenter - mid_BC[0]) + mid_BC[1]
    else:  # BC is vertical
        x_circumcenter = mid_BC[0]
        y_circumcenter = perp_slope_AB * (x_circumcenter - mid_AB[0]) + mid_AB[1]
    
    return (x_circumcenter, y_circumcenter)

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

timestamp = int(time.time())
veh_E_name = "Drone0"  # evader
veh_P1_name = "Drone1"  # P1
veh_P2_name = "Drone2"  # P2
veh_P3_name = "Drone3"  # P3

# Enable API control for both vehicles
client.enableApiControl(True, veh_E_name)
client.enableApiControl(True, veh_P1_name)
client.enableApiControl(True, veh_P2_name)
client.enableApiControl(True, veh_P3_name)

client.armDisarm(True, vehicle_name=veh_E_name)
client.armDisarm(True, vehicle_name=veh_P1_name)
client.armDisarm(True, vehicle_name=veh_P2_name)
client.armDisarm(True, vehicle_name=veh_P3_name)


# TAKE OFF

client.takeoffAsync(vehicle_name=veh_E_name).join()
client.takeoffAsync(vehicle_name=veh_P1_name).join()
client.takeoffAsync(vehicle_name=veh_P2_name).join()
client.takeoffAsync(vehicle_name=veh_P3_name).join()

ht = 17 ## USE THIS FOR Z COORDINATE EVERYWHERE
init_position_E = (0, 0)
init_position_P = [(-7, -8), (-9, 13), (3, 3)]

# Init Movement

client.moveToPositionAsync(init_position_E[0], init_position_E[1], ht , 1 , vehicle_name = "Drone0" ).join()

for i in range(3):  
    client.moveToPositionAsync(init_position_P[i][0], init_position_P[i][1], ht, 1,  vehicle_name=f"Drone{i+1}").join()


far_P_index = 0
for i in range(100000):
    time.sleep(1)
    print(f"{i} - ITERATION")
    e =   client.simGetObjectPose(veh_E_name).position
    p =  [client.simGetObjectPose(veh_P1_name).position, client.simGetObjectPose(veh_P2_name).position, client.simGetObjectPose(veh_P3_name).position]

    print(f"Evader Drone Ground Truth Position: {e.x_val}, {e.y_val}, {e.z_val}")
    print(f"P1 Drone Ground Truth Position: {p[0].x_val}, {p[0].y_val}, {p[0].z_val}")
    print(f"P2 Drone Ground Truth Position: {p[1].x_val}, {p[1].y_val}, {p[1].z_val}")
    print(f"P3 Drone Ground Truth Position: {p[2].x_val}, {p[2].y_val}, {p[2].z_val}")

    rel_mag_wrt_e = [mag_3d(p[0]-e), mag_3d(p[1]-e), mag_3d(p[2]-e)]
    far_P_index = rel_mag_wrt_e.index(max(rel_mag_wrt_e)) + 1 

    print(f"Farthest: Drone{far_P_index}")
    # PURE PURSUIT FAR DRONE
    client.moveToPositionAsync(e.x_val, e.y_val, ht, 2,
                                vehicle_name=f"Drone{far_P_index}")

    x = circumcenter_2d((e.x_val, e.y_val), (p[(far_P_index)%3].x_val, p[(far_P_index)%3].y_val), (p[(far_P_index+1)%3].x_val, p[(far_P_index+1)%3].y_val))
    print(f"Circumcenter: {x, ht}")

    # MOVEMENT NEAR DRONES AND EVADER
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name=f"Drone{(far_P_index+1)%3}")
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name=f"Drone{(far_P_index+2)%3}")
    client.moveToPositionAsync(x[0], x[1], ht, 2,
                                vehicle_name="Drone0")
    
    # Store data in CSV format at the end of each iteration (no parentheses)
    data = {
        "Evader_X": [e.x_val],  # Evader X position
        "Evader_Y": [e.y_val],  # Evader Y position
        "Pursuer1_X": [p[0].x_val],  # Pursuer 1 X position
        "Pursuer1_Y": [p[0].y_val],  # Pursuer 1 Y position
        "Pursuer2_X": [p[1].x_val],  # Pursuer 2 X position
        "Pursuer2_Y": [p[1].y_val],  # Pursuer 2 Y position
        "Pursuer3_X": [p[2].x_val],  # Pursuer 3 X position
        "Pursuer3_Y": [p[2].y_val],  # Pursuer 3 Y position
    }

    # Convert to DataFrame and append to CSV
    df = pd.DataFrame(data)
    csv_file = "drone_positions.csv"
    if i == 0:
        df.to_csv(csv_file, mode='w', header=True, index=False)  # Write headers on the first iteration
    else:
        df.to_csv(csv_file, mode='a', header=False, index=False)  # Append data without headers

settings.json used:

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


  "Vehicles": {
    "Drone0": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": 0,
      "Y": 0,
      "Z": 0
    },
    "Drone1": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": -2,
      "Y": -2,
      "Z": 0
    },
    "Drone2": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": -2,
      "Y": 3,
      "Z": 0
    },
    "Drone3": {
      "VehicleType": "SimpleFlight",
      "AllowAPIAlways": true,
      "X": 3,
      "Y": 3,
      "Z": 0
    }
  }
}