nothing is woking in the below code

import time
import numpy as np
from pymavlink import mavutil
from multiprocessing import Process, Manager
import random
from collections import deque

class Drone:
    def __init__(self, sysid):
        self.sysid = sysid
        self.current_pos = np.zeros(3)

class SwarmDroneController:
    def __init__(self, master_sysid, follower_sysids):
        self.master = Drone(master_sysid)
        self.followers = [Drone(sysid) for sysid in follower_sysids]
        self.connection = mavutil.mavlink_connection('/dev/ttyUSB0')
        
        self.current_ned = np.zeros(3)
        self.waypoints = []
        self.distance_map = Manager().dict() # for swarm communication and distance to master
        self.state_rewards = deque(maxlen=1000)

    def set_mode(self, drone, mode):
        # self.connection.mav.set_mode_send(drone.sysid, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode)
        self.connection.mav.set_mode_send(
            drone.sysid,
            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
            mode
        )
        time.sleep(1)

    def arm_and_takeoff(self, drone, altitude):
        self.connection.mav.command_long_send(
        drone.sysid, mavutil.mavlink.MAV_COMP_ID_SYSTEM_CONTROL,
        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)
        
        time.sleep(1)

        self.connection.mav.command_long_send(
        drone.sysid, mavutil.mavlink.MAV_COMP_ID_SYSTEM_CONTROL,
        mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude)
        self.connection.wait_heartbeat()
        time.sleep(5)

    def add_waypoint(self, waypoint):
        self.waypoints.append(waypoint)

    def send_position_target(self, drone, ned_pos):
        self.connection.mav.set_position_target_local_ned_send(
        0, 
        drone.sysid, 
        0, 
        mavutil.mavlink.MAV_FRAME_LOCAL_NED,
        int(0b110111111000),
        ned_pos[0], 
        ned_pos[1], 
        ned_pos[2], 
        0, 
        0, 
        0, 
        0, 
        0, 
        0,
        0,
        0)
        time.sleep(0.1)

    def goto_next_waypoint(self):
        if len(self.waypoints) == 0:
            return False

        next_global_waypoint = self.waypoints.pop(0)
        self.send_position_target(self.master, self.current_ned + next_global_waypoint)

        return True

    def follow_position(self, follower, target_pos):
        while True:
            follower.current_pos = target_pos
            self.send_position_target(follower, self.current_ned + target_pos)
            time.sleep(0.1)

    def formation_follow_master(self, formation_coords):
        processes = []
        for i, follower in enumerate(self.followers):
            target_coords = np.array(self.master.current_pos) + formation_coords[i]
            process = Process(target=self.follow_position, args=(follower, target_coords))
            processes.append(process)
            process.start()

        try:
            while self.goto_next_waypoint():
                for i, process in enumerate(processes):
                    process.join(timeout=1)
        finally:
            for process in processes:
                process.terminate()

    def update_local_positions(self):
        local_position = self.connection.messages.get("LOCAL_POSITION_NED", None)
        if local_position is not None:
            self.master.current_pos = np.array([local_position.x, local_position.y, local_position.z])

    def reward_func(self, follower):
        # Example reward for a follower maintaining within a certain distance to master
        dist = np.linalg.norm(self.master.current_pos - follower.current_pos)
        within_distance = 5.0 # Treshold distance in meters
        if dist <= within_distance:
            return 1.0
        else:
            return -1.0

    def update_state(self, follower):
        reward = self.reward_func(follower)
        self.state_rewards.append((follower.sysid, reward))

    def exploration(self, formation_coords):
        """
        Reinforcement Learning approach for exploration of different positions.
        """
        epsilon = 0.1 # exploration rate
        best_coords = formation_coords
        best_reward = -float('inf')
        for _ in range(10): # 10 random samples
            candidate_coords = best_coords.copy()
            for coord in candidate_coords:
                if random.random() < epsilon:
                    coord += random.uniform(-1, 1) # random deviation in meters
            mean_reward = self.evaluate_position(candidate_coords)
            if mean_reward > best_reward:
                best_reward = mean_reward
                best_coords = candidate_coords
        return best_coords
    def evaluate_position(self, formation_coords):
        """
        Evaluate formation coordinates by creating a temporary formation and
        measuring the reward for each follower.
        """
        self.formation_follow_master(formation_coords)
        rewards = []
        for follower in self.followers:
            self.update_state(follower)
            rewards.append(self.reward_func(follower))
        self.set_mode(self.master, 4) # Set mode to Guided
        for follower in self.followers:
            self.set_mode(follower, 4)
        return np.mean(rewards)

# Usage
formation_shape = np.array([-5, 0, 0])
controller = SwarmDroneController(2, [3])
print("formation done")

# Arm and takeoff both drones
controller.set_mode(controller.master, 4) # Set mode to Guided
for follower in controller.followers:
    controller.set_mode(follower, 4)


controller.arm_and_takeoff(controller.master, 10)
for follower in controller.followers:
    controller.arm_and_takeoff(follower, 10)        
print("arma and takeoff done")
# Add waypoints
controller.add_waypoint(np.array([0, 10.0, -10.0]))
controller.add_waypoint(np.array([0, 20.0, -10.0]))

# Exploration of formation shape using reinforcement learning
formation_shape = controller.exploration(formation_shape)

# Send master drone to waypoints, followers follow in formation
controller.formation_follow_master(formation_shape)
print("waypoints send")

# After the operation has completed, set mode to RTL and disarm
controller.set_mode(controller.master, 6) # Set mode to RTL
for follower in controller.followers:
    controller.set_mode(follower, 6)
print("mode set to rtl")

controller.arm_and_takeoff(controller.master, 0)
for follower in controller.followers:
    controller.arm_and_takeoff(follower, 0)
print("drone disarmed")



not setting the mode to guided,arm,takeofff, mission waypoints, setting the mdoe to rtl,disarming the drone,

please give me full modified and working code