'multiple processing in dronekit not working

i am trying to make a code about a drone flying to multiple waypoint and the drone can't continue to the next waypoint when i not showing the red color on camera.

because the camera cv2 and the drone runs at the same time, my code runs very laggy, so i tried using multiprocessing method and modify my code. when i trying to run my new code, my multi processing doesn't work and it keeps skipping almost of my code and straight to RTL mode.

from inspect import ArgInfo
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
from numpy import loadtxt, array
from time import sleep
import sys
import cv2
import numpy as np
import multiprocessing

cap = cv2.VideoCapture(0)
hsv_a = np.array([198, 255, 255])
hsv_b = np.array([158, 68, 137])
treshold = 150
lat = [-35.3629722, -35.3629064, -35.3634361, -35.3638474]
lon = [149.1649709, 149.1655721, 149.1657331, 149.1639733]


#vehicle = connect('udp:127.0.0.1:14551',wait_ready=True)
vehicle = connect('udp:127.0.0.1:14551',wait_ready=True)


def arm_and_takeoff(aTargetAltitude): #fungsi arming dan takeoff
    print("Basic pre-arm checks")
    # Don't let the user try to arm until autopilot is ready
    while not(vehicle.is_armable):
        print(" Waiting for vehicle to initialise...")
        sleep(1)

    print("Arming motors")
    # Copter should arm in GUIDED mode
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True

    while not(vehicle.armed):
        print(" Waiting for arming...")
        sleep(1)

    print("Taking off!")
    vehicle.simple_takeoff(aTargetAltitude)
    
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)
        #Break and return from function just below target altitude.
        if (vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95):
            print("Reached target altitude")
            break
        sleep(1)

def dist(a,z): #a=awal z=akhir
    d_lat= (a.lat-z.lat)**2
    d_long= (a.lon-z.lon)**2
    jarak = (d_lat+d_long)**0.5
    return jarak

def gerak_drone():
    for i in range(0,len(lat)):
        print(i)
        wp = LocationGlobalRelative(lat[i],lon[i],2)
        vehicle.simple_goto(wp)
        sleep(1)
        
        while (dist(vehicle.location.global_relative_frame,wp)>=0.0001):
            print (str(round(dist(vehicle.location.global_relative_frame,wp)*100000,2)))
            while True:
                _,frame = cap.read()
    
                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                mask = cv2.inRange(hsv, hsv_b, hsv_a)

                cv2.imshow("warna", mask)
                cv2.imshow("hitamPutih", gray)
                cv2.imshow("apa", frame)

                print(cv2.countNonZero(mask))
    
                if cv2.waitKey(500) == 27 or cv2.countNonZero(mask) > treshold : 
                    break


if __name__ == "_main_":
    altitude = 2
    lat_distance = 1
    lon_distance = 1

    p1 = multiprocessing.Process(target=arm_and_takeoff, args=(altitude))
    p2 = multiprocessing.Process(target=dist, args=(lat_distance, lon_distance))
    p3 = multiprocessing.Process(target=gerak_drone)

    p1.start()
    p2.start()
    p3.start()
    
    p1.join()
    p2.join()
    p3.join()

        
print("Coming back")
vehicle.mode = VehicleMode("RTL")

sleep(20)

vehicle.mode = VehicleMode("LAND")

Here is my terminal result



Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source