├── CircleDetection.py ├── Plane.py └── README.md /CircleDetection.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, Battery, LocationGlobal, Attitude 2 | from pymavlink import mavutil 3 | 4 | import time 5 | import math 6 | import numpy as np 7 | import psutil 8 | import argparse 9 | import copy 10 | import cv2 11 | import numpy as np 12 | import keyboard 13 | from Plane import Plane 14 | 15 | if __name__ == '__main__': 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument('--connect', default='tcp:127.0.0.1:5762') 18 | args = parser.parse_args() 19 | 20 | 21 | connection_string = args.connect 22 | 23 | #-- Create the object 24 | plane = Plane(connection_string) 25 | 26 | #-- Arm and takeoff 27 | if not plane.is_armed(): plane.arm_and_takeoff() 28 | 29 | 30 | def nothing(x): 31 | pass 32 | 33 | cap = cv2.VideoCapture(0) 34 | cv2.namedWindow("Settings") 35 | cv2.createTrackbar("Lower-Hue", "Settings", 106, 180, nothing) 36 | cv2.createTrackbar("Lower-Saturation", "Settings", 118, 255, nothing) 37 | cv2.createTrackbar("Lower-Value", "Settings", 0, 255, nothing) 38 | cv2.createTrackbar("Upper-Hue", "Settings", 180, 180, nothing) 39 | cv2.createTrackbar("Upper-Saturation", "Settings", 255, 255, nothing) 40 | cv2.createTrackbar("Upper-Value", "Settings", 255, 255, nothing) 41 | 42 | font = cv2.FONT_HERSHEY_SIMPLEX 43 | 44 | while True: 45 | ret, frame = cap.read() 46 | hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 47 | frame = cv2.flip(frame, 1) 48 | 49 | lh = cv2.getTrackbarPos("Lower-Hue", "Settings") 50 | ls = cv2.getTrackbarPos("Lower-Saturation", "Settings") 51 | lv = cv2.getTrackbarPos("Lower-Value", "Settings") 52 | 53 | uh = cv2.getTrackbarPos("Upper-Hue", "Settings") 54 | us = cv2.getTrackbarPos("Upper-Saturation", "Settings") 55 | uv = cv2.getTrackbarPos("Upper-Value", "Settings") 56 | 57 | lower_color = np.array([lh, ls, lv]) 58 | upper_color = np.array([uh, us, uv]) 59 | 60 | mask = cv2.inRange(hsv, lower_color, upper_color) 61 | kernel = np.ones((5, 5), np.uint8) 62 | mask = cv2.erode(mask, kernel) 63 | mask = cv2.flip(mask, 1) 64 | 65 | contours,_ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 66 | 67 | for cnt in contours: 68 | area = cv2.contourArea(cnt) 69 | epsilon = 0.02 * cv2.arcLength(cnt, True) 70 | approx = cv2.approxPolyDP(cnt, epsilon, True) 71 | 72 | x = approx.ravel()[0] 73 | y = approx.ravel()[1] 74 | 75 | if area > 400: 76 | cv2.drawContours(frame, [approx], 0, (0, 255, 0), 2) 77 | if len(approx) > 6: 78 | cv2.putText(frame, "HEDEF", (x, y), font, 1, (0, 0, 255)) 79 | 80 | 81 | if cv2.contourArea(cnt) <= 50: 82 | continue 83 | x,y,w,h = cv2.boundingRect(cnt) 84 | #cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255,0), 2) 85 | center = (x,y) 86 | if x > 300: 87 | plane.go_to_waypoint2() 88 | print("Gidiyor waypoint 2") 89 | elif x < 300: 90 | plane.go_to_waypoint1() 91 | print("Gidiyor waypoint 1") 92 | 93 | 94 | 95 | cv2.resizeWindow("frame", 600, 450) 96 | #cv2.resizeWindow("mask", 600, 450) 97 | cv2.imshow("frame", frame) 98 | #cv2.imshow("mask", mask) 99 | 100 | if cv2.waitKey(1) == 27: 101 | break 102 | 103 | cap.release() 104 | cv2.destroyAllWindows() 105 | 106 | -------------------------------------------------------------------------------- /Plane.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, Battery, LocationGlobal, Attitude 2 | from pymavlink import mavutil 3 | 4 | import time 5 | import math 6 | import numpy as np 7 | import psutil 8 | import argparse 9 | import copy 10 | 11 | 12 | def nothing(x): 13 | pass 14 | 15 | class Plane(): 16 | 17 | def __init__(self, connection_string=None, vehicle=None): 18 | """ Initialize the object 19 | Use either the provided vehicle object or the connections tring to connect to the autopilot 20 | 21 | Input: 22 | connection_string - the mavproxy style connection string, like tcp:127.0.0.1:5760 23 | default is None 24 | vehicle - dronekit vehicle object, coming from another instance (default is None) 25 | 26 | 27 | """ 28 | 29 | #---- Connecting with the vehicle, using either the provided vehicle or the connection string 30 | if not vehicle is None: 31 | self.vehicle = vehicle 32 | print("Using the provided vehicle") 33 | elif not connection_string is None: 34 | 35 | print("Connecting with vehicle...") 36 | self._connect(connection_string) 37 | else: 38 | raise("ERROR: a valid dronekit vehicle or a connection string must be supplied") 39 | return 40 | 41 | self.wind_dir_to_deg = 0.0 #- [deg] wind direction (where it is going) 42 | self.wind_dir_from_deg = 0.0 #- [deg] wind coming from direction 43 | self.wind_speed = 0.0 #- [m/s] wind speed 44 | 45 | self.climb_rate = 0.0 #- [m/s] climb rate 46 | self.throttle = 0.0 #- [ ] throttle (0-100) 47 | 48 | self._setup_listeners() 49 | 50 | self.airspeed = 0.0 #- [m/s] airspeed 51 | self.groundspeed = 0.0 #- [m/s] ground speed 52 | 53 | self.pos_lat = 0.0 #- [deg] latitude 54 | self.pos_lon = 0.0 #- [deg] longitude 55 | self.pos_alt_rel = 0.0 #- [m] altitude relative to takeoff 56 | self.pos_alt_abs = 0.0 #- [m] above mean sea level 57 | 58 | self.att_roll_deg = 0.0 #- [deg] roll 59 | self.att_pitch_deg = 0.0 #- [deg] pitch 60 | self.att_heading_deg = 0.0 #- [deg] magnetic heading 61 | 62 | self.ap_mode = '' #- [] Autopilot flight mode 63 | 64 | self.mission = self.vehicle.commands #-- mission items 65 | 66 | self.location_home = LocationGlobalRelative(0,0,0) #- LocationRelative type home 67 | self.location_current = LocationGlobalRelative(0,0,0) #- LocationRelative type current position 68 | 69 | def Throttle(): 70 | n_tries = 0 71 | while not self.vehicle.armed: 72 | print("Try to arm...") 73 | self.arm() 74 | n_tries += 1 75 | time.sleep(2.0) 76 | 77 | if n_tries > 5: 78 | print("!!! CANNOT ARM") 79 | break 80 | self.throttle = 50 81 | 82 | def _connect(self, connection_string): #-- (private) Connect to Vehicle 83 | """ (private) connect with the autopilot 84 | 85 | Input: 86 | connection_string - connection string (mavproxy style) 87 | """ 88 | self.vehicle = connect(connection_string, wait_ready=True, heartbeat_timeout=60) 89 | self._setup_listeners() 90 | 91 | def _setup_listeners(self): #-- (private) Set up listeners 92 | #---------------------------- 93 | #--- CALLBACKS 94 | #---------------------------- 95 | if True: 96 | #---- DEFINE CALLBACKS HERE!!! 97 | @self.vehicle.on_message('ATTITUDE') 98 | def listener(vehicle, name, message): #--- Attitude 99 | self.att_roll_deg = math.degrees(message.roll) 100 | self.att_pitch_deg = math.degrees(message.pitch) 101 | self.att_heading_deg = math.degrees(message.yaw)%360 102 | 103 | @self.vehicle.on_message('GLOBAL_POSITION_INT') 104 | def listener(vehicle, name, message): #--- Position / Velocity 105 | self.pos_lat = message.lat*1e-7 106 | self.pos_lon = message.lon*1e-7 107 | self.pos_alt_rel = message.relative_alt*1e-3 108 | self.pos_alt_abs = message.alt*1e-3 109 | self.location_current = LocationGlobalRelative(self.pos_lat, self.pos_lon, self.pos_alt_rel) 110 | 111 | 112 | @self.vehicle.on_message('VFR_HUD') 113 | def listener(vehicle, name, message): #--- HUD 114 | self.airspeed = message.airspeed 115 | self.groundspeed = message.groundspeed 116 | self.throttle = message.throttle 117 | self.climb_rate = message.climb 118 | 119 | @self.vehicle.on_message('WIND') 120 | def listener(vehicle, name, message): #--- WIND 121 | self.wind_speed = message.speed 122 | self.wind_dir_from_deg = message.direction % 360 123 | self.wind_dir_to_deg = (self.wind_dir_from_deg + 180) % 360 124 | 125 | 126 | return (self.vehicle) 127 | print(">> Connection Established") 128 | 129 | def _get_location_metres(self, original_location, dNorth, dEast, is_global=False): 130 | """ 131 | Returns a Location object containing the latitude/longitude `dNorth` and `dEast` metres from the 132 | specified `original_location`. The returned Location has the same `alt and `is_relative` values 133 | as `original_location`. 134 | 135 | The function is useful when you want to move the vehicle around specifying locations relative to 136 | the current vehicle position. 137 | The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles. 138 | For more information see: 139 | http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters 140 | """ 141 | earth_radius=6378137.0 #Radius of "spherical" earth 142 | #Coordinate offsets in radians 143 | dLat = dNorth/earth_radius 144 | dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180)) 145 | 146 | #New position in decimal degrees 147 | newlat = original_location.lat + (dLat * 180/math.pi) 148 | newlon = original_location.lon + (dLon * 180/math.pi) 149 | 150 | if is_global: 151 | return LocationGlobal(newlat, newlon,original_location.alt) 152 | else: 153 | return LocationGlobalRelative(newlat, newlon,original_location.alt) 154 | 155 | def is_armed(self): #-- Check whether uav is armed 156 | """ Checks whether the UAV is armed 157 | 158 | """ 159 | return(self.vehicle.armed) 160 | 161 | def arm(self): #-- arm the UAV 162 | """ Arm the UAV 163 | """ 164 | self.vehicle.armed = True 165 | 166 | def disarm(self): #-- disarm UAV 167 | """ Disarm the UAV 168 | """ 169 | self.vehicle.armed = False 170 | 171 | def set_airspeed(self, speed): #--- Set target airspeed 172 | """ Set uav airspeed m/s 173 | """ 174 | self.vehicle.airspeed = speed 175 | 176 | def set_ap_mode(self, mode): #--- Set Autopilot mode 177 | """ Set Autopilot mode 178 | """ 179 | time_0 = time.time() 180 | try: 181 | tgt_mode = VehicleMode(mode) 182 | except: 183 | return(False) 184 | 185 | while (self.get_ap_mode() != tgt_mode): 186 | self.vehicle.mode = tgt_mode 187 | time.sleep(0.2) 188 | if time.time() < time_0 + 5: 189 | return (False) 190 | 191 | return (True) 192 | 193 | def get_ap_mode(self): #--- Get the autopilot mode 194 | """ Get the autopilot mode 195 | """ 196 | self._ap_mode = self.vehicle.mode 197 | return(self.vehicle.mode) 198 | 199 | def clear_mission(self): #--- Clear the onboard mission 200 | """ Clear the current mission. 201 | 202 | """ 203 | cmds = self.vehicle.commands 204 | self.vehicle.commands.clear() 205 | self.vehicle.flush() 206 | 207 | # After clearing the mission you MUST re-download the mission from the vehicle 208 | # before vehicle.commands can be used again 209 | # (see https://github.com/dronekit/dronekit-python/issues/230) 210 | self.mission = self.vehicle.commands 211 | self.mission.download() 212 | self.mission.wait_ready() 213 | 214 | def download_mission(self): #--- download the mission 215 | """ Download the current mission from the vehicle. 216 | 217 | """ 218 | self.vehicle.commands.download() 219 | self.vehicle.commands.wait_ready() # wait until download is complete. 220 | self.mission = self.vehicle.commands 221 | 222 | def mission_add_takeoff(self, takeoff_altitude=50, takeoff_pitch=15, heading=None): 223 | """ Adds a takeoff item to the UAV mission, if it's not defined yet 224 | 225 | Input: 226 | takeoff_altitude - [m] altitude at which the takeoff is considered over 227 | takeoff_pitch - [deg] pitch angle during takeoff 228 | heading - [deg] heading angle during takeoff (default is the current) 229 | """ 230 | if heading is None: heading = self.att_heading_deg 231 | 232 | self.download_mission() 233 | #-- save the mission: copy in the memory 234 | tmp_mission = list(self.mission) 235 | 236 | # print tmp_mission.count 237 | is_mission = False 238 | if len(tmp_mission) >= 1: 239 | is_mission = True 240 | print("Current mission:") 241 | #for item in tmp_mission: 242 | # print item 243 | #-- If takeoff already in the mission, do not do anything 244 | 245 | if is_mission and tmp_mission[0].command == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: 246 | print ("Takeoff already in the mission") 247 | else: 248 | print("Takeoff not in the mission: adding") 249 | self.clear_mission() 250 | takeoff_item = Command( 0, 0, 0, 3, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, takeoff_pitch, 0, 0, 0, 0, 0, takeoff_altitude) 251 | self.mission.add(takeoff_item) 252 | for item in tmp_mission: 253 | self.mission.add(item) 254 | self.vehicle.flush() 255 | print(">>>>>Done") 256 | 257 | def arm_and_takeoff(self, altitude=100, pitch_deg=40): 258 | """ Arms the UAV and takeoff 259 | Planes need a takeoff item in the mission and to be set into AUTO mode. The 260 | heading is kept constant 261 | 262 | Input: 263 | altitude - altitude at which the takeoff is concluded 264 | pitch_deg - pitch angle during takeoff 265 | """ 266 | 267 | self.mission_add_takeoff(takeoff_altitude=1.5*altitude, takeoff_pitch=pitch_deg) 268 | print ("Takeoff mission ready") 269 | 270 | while not self.vehicle.is_armable: 271 | print("Wait to be armable...") 272 | time.sleep(1.0) 273 | 274 | 275 | #-- Save home 276 | while self.pos_lat == 0.0: 277 | time.sleep(0.5) 278 | print ("Waiting for good GPS...") 279 | self.location_home = LocationGlobalRelative(self.pos_lat,self.pos_lon,altitude) 280 | 281 | print("Home is saved as "), self.location_home 282 | print ("Vehicle is Armable: try to arm") 283 | self.set_ap_mode("MANUAL") 284 | n_tries = 0 285 | while not self.vehicle.armed: 286 | print("Try to arm...") 287 | self.arm() 288 | n_tries += 1 289 | time.sleep(2.0) 290 | 291 | if n_tries > 5: 292 | print("!!! CANNOT ARM") 293 | break 294 | 295 | #--- Set to auto and check the ALTITUDE 296 | 297 | if self.vehicle.armed: 298 | print ("ARMED") 299 | self.set_ap_mode("AUTO") 300 | 301 | while self.pos_alt_rel <= altitude - 20.0: 302 | print ("Altitude = %.0f"%self.pos_alt_rel) 303 | time.sleep(2.0) 304 | 305 | print("Altitude reached: set to GUIDED") 306 | self.set_ap_mode("GUIDED") 307 | 308 | time.sleep(1.0) 309 | 310 | waypoint3 = LocationGlobalRelative(53.3833284, 30.8496094, 100) 311 | self.vehicle.simple_goto(waypoint3) 312 | 313 | """ 314 | if keyboard.is_pressed('q'): 315 | self.vehicle.simple_goto(waypoint1) 316 | elif keyboard.is_pressed('e'): 317 | self.vehicle.simple_goto(waypoint2) 318 | """ 319 | 320 | return True 321 | 322 | def go_to_waypoint1(self): 323 | waypoint1 = LocationGlobalRelative(46.8000594, 14.1943359, 100) 324 | self.vehicle.simple_goto(waypoint1) 325 | 326 | def go_to_waypoint2(self): 327 | waypoint2 = LocationGlobalRelative(46.4983923, 47.9003906, 100) 328 | self.vehicle.simple_goto(waypoint2) 329 | 330 | def get_target_from_bearing(self, original_location, ang, dist, altitude=None): 331 | """ Create a TGT request packet located at a bearing and distance from the original point 332 | 333 | Inputs: 334 | ang - [rad] Angle respect to North (clockwise) 335 | dist - [m] Distance from the actual location 336 | altitude- [m] 337 | Returns: 338 | location - Dronekit compatible 339 | """ 340 | 341 | if altitude is None: altitude = original_location.alt 342 | 343 | # print '---------------------- simulate_target_packet' 344 | dNorth = dist*math.cos(ang) 345 | dEast = dist*math.sin(ang) 346 | # print "Based on the actual heading of %.0f, the relative target's coordinates are %.1f m North, %.1f m East" % (math.degrees(ang), dNorth, dEast) 347 | 348 | #-- Get the Lat and Lon 349 | tgt = self._get_location_metres(original_location, dNorth, dEast) 350 | 351 | tgt.alt = altitude 352 | # print "Obtained the following target", tgt.lat, tgt.lon, tgt.alt 353 | 354 | return tgt 355 | 356 | def ground_course_2_location(self, angle_deg, altitude=None): 357 | """ Creates a target to aim to in order to follow the ground course 358 | Input: 359 | angle_deg - target ground course 360 | altitude - target altitude (default the current) 361 | 362 | """ 363 | tgt = self.get_target_from_bearing(original_location=self.location_current, 364 | ang=math.radians(angle_deg), 365 | dist=5000, 366 | altitude=altitude) 367 | return(tgt) 368 | 369 | def goto(self, location): 370 | """ Go to a location 371 | 372 | Input: 373 | location - LocationGlobal or LocationGlobalRelative object 374 | 375 | """ 376 | self.vehicle.simple_goto(location) 377 | 378 | def set_ground_course(self, angle_deg, altitude=None): 379 | """ Set a ground course 380 | 381 | Input: 382 | angle_deg - [deg] target heading 383 | altitude - [m] target altitude (default the current) 384 | 385 | """ 386 | 387 | #-- command the angles directly 388 | self.goto(self.ground_course_2_location(angle_deg, altitude)) 389 | 390 | def get_rc_channel(self, rc_chan, dz=0, trim=1500): #--- Read the RC values from the channel 391 | """ 392 | Gets the RC channel values with a dead zone around trim 393 | 394 | Input: 395 | rc_channel - input rc channel number 396 | dz - dead zone, within which the output is set equal to trim 397 | trim - value about which the dead zone is evaluated 398 | 399 | Returns: 400 | rc_value - [us] 401 | """ 402 | if (rc_chan > 16 or rc_chan < 1): 403 | return -1 404 | 405 | #- Find the index of the channel 406 | strInChan = '%1d' % rc_chan 407 | try: 408 | 409 | rcValue = int(self.vehicle.channels.get(strInChan)) 410 | 411 | if dz > 0: 412 | if (math.fabs(rcValue - trim) < dz): 413 | return trim 414 | 415 | return rcValue 416 | except: 417 | return 0 418 | 419 | def set_rc_channel(self, rc_chan, value_us=0): #--- Overrides a rc channel (call with no value to reset) 420 | """ 421 | Overrides the RC input setting the provided value. Call with no value to reset 422 | 423 | Input: 424 | rc_chan - rc channel number 425 | value_us - pwm value 426 | """ 427 | strInChan = '%1d' % rc_chan 428 | self.vehicle.channels.overrides[strInChan] = int(value_us) 429 | 430 | def clear_all_rc_override(self): #--- clears all the rc channel override 431 | self.vehicle.channels.overrides = {} 432 | 433 | 434 | 435 | 436 | if __name__ == '__main__': 437 | parser = argparse.ArgumentParser() 438 | parser.add_argument('--connect', default='tcp:127.0.0.1:5762') 439 | args = parser.parse_args() 440 | 441 | 442 | connection_string = args.connect 443 | 444 | #-- Create the object 445 | plane = Plane(connection_string) 446 | 447 | #-- Arm and takeoff 448 | if not plane.is_armed(): plane.arm_and_takeoff() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Plane-Simulation-OpenCV-Dronekit-Python 2 | 3 | Autonomous UAV with image processing on Mission Planner 4 | 5 | https://www.youtube.com/watch?v=LeJycUjXMA8 6 | 7 | ![Screenshot_9](https://user-images.githubusercontent.com/79511355/158851929-794d39fd-a8d0-4025-a324-793065ac823d.png) 8 | 9 | ## Usage 10 | 11 | - Install Libraries and 12 | 13 | ``` 14 | python CircleDetection.py 15 | ``` 16 | --------------------------------------------------------------------------------