1+ import numpy as np
2+ import time
3+ import os
4+ import pprint
5+ import tempfile
6+ import math
7+ from math import *
8+
9+ from projectairsim import ProjectAirSimClient , Drone , World
10+ from projectairsim .utils import projectairsim_log , rpy_to_quaternion , unpack_image
11+ from projectairsim .types import ImageType , Vector3 , Quaternion , Pose
12+
13+ from abc import ABC , abstractmethod
14+
15+ def to_quaternion (pitch , roll , yaw ):
16+ quat = rpy_to_quaternion (pitch , roll , yaw )
17+ return Quaternion ({"w" : quat [0 ], "x" : quat [1 ], "y" : quat [2 ], "z" : quat [3 ]})
18+
19+ #define abstract class to return next vector in the format (x,y,yaw)
20+ class AbstractClassGetNextVec (ABC ):
21+
22+ @abstractmethod
23+ def get_next_vec (self , depth , obj_sz , goal , pos ):
24+ print ("Some implementation!" )
25+ return pos ,yaw
26+
27+ class ReactiveController (AbstractClassGetNextVec ):
28+ def get_next_vec (self , depth , obj_sz , goal , pos ):
29+ print ("Some implementation!" )
30+ return
31+
32+ class AvoidLeft (AbstractClassGetNextVec ):
33+
34+ def __init__ (self , hfov = radians (90 ), coll_thres = 5 , yaw = 0 , limit_yaw = 5 , step = 0.1 ):
35+ self .hfov = hfov
36+ self .coll_thres = coll_thres
37+ self .yaw = yaw
38+ self .limit_yaw = limit_yaw
39+ self .step = step
40+
41+ def get_next_vec (self , depth , obj_sz , goal , pos ):
42+
43+ [h ,w ] = np .shape (depth )
44+ [roi_h ,roi_w ] = compute_bb ((h ,w ), obj_sz , self .hfov , self .coll_thres )
45+
46+ # compute vector, distance and angle to goal
47+ t_vec , t_dist , t_angle = get_vec_dist_angle (goal , pos [:- 1 ])
48+
49+ # compute box of interest
50+ img2d_box = img2d [int ((h - roi_h )/ 2 ):int ((h + roi_h )/ 2 ),int ((w - roi_w )/ 2 ):int ((w + roi_w )/ 2 )]
51+
52+ # scale by weight matrix (optional)
53+ #img2d_box = np.multiply(img2d_box,w_mtx)
54+
55+ # detect collision
56+ if (np .min (img2d_box ) < coll_thres ):
57+ self .yaw = self .yaw - radians (self .limit_yaw )
58+ else :
59+ self .yaw = self .yaw + min (t_angle - self .yaw , radians (self .limit_yaw ))
60+
61+ pos [0 ] = pos [0 ] + self .step * cos (self .yaw )
62+ pos [1 ] = pos [1 ] + self .step * sin (self .yaw )
63+
64+ return pos , self .yaw ,t_dist
65+
66+ class AvoidLeftIgonreGoal (AbstractClassGetNextVec ):
67+
68+ def __init__ (self , hfov = radians (90 ), coll_thres = 5 , yaw = 0 , limit_yaw = 5 , step = 0.1 ):
69+ self .hfov = hfov
70+ self .coll_thres = coll_thres
71+ self .yaw = yaw
72+ self .limit_yaw = limit_yaw
73+ self .step = step
74+
75+ def get_next_vec (self , depth , obj_sz , goal , pos ):
76+
77+ [h ,w ] = np .shape (depth )
78+ [roi_h ,roi_w ] = compute_bb ((h ,w ), obj_sz , self .hfov , self .coll_thres )
79+
80+ # compute box of interest
81+ img2d_box = img2d [int ((h - roi_h )/ 2 ):int ((h + roi_h )/ 2 ),int ((w - roi_w )/ 2 ):int ((w + roi_w )/ 2 )]
82+
83+ # detect collision
84+ if (np .min (img2d_box ) < coll_thres ):
85+ self .yaw = self .yaw - radians (self .limit_yaw )
86+
87+ pos [0 ] = pos [0 ] + self .step * cos (self .yaw )
88+ pos [1 ] = pos [1 ] + self .step * sin (self .yaw )
89+
90+ return pos , self .yaw , 100
91+
92+ #compute resultant normalized vector, distance and angle
93+ def get_vec_dist_angle (goal , pos ):
94+ vec = np .array (goal ) - np .array (pos )
95+ dist = math .sqrt (vec [0 ]** 2 + vec [1 ]** 2 )
96+ angle = math .atan2 (vec [1 ],vec [0 ])
97+ if angle > math .pi :
98+ angle -= 2 * math .pi
99+ elif angle < - math .pi :
100+ angle += 2 * math .pi
101+ return vec / dist , dist , angle
102+
103+ def get_local_goal (v , pos , theta ):
104+ return goal
105+
106+ #compute bounding box size
107+ def compute_bb (image_sz , obj_sz , hfov , distance ):
108+ vfov = hfov2vfov (hfov ,image_sz )
109+ box_h = ceil (obj_sz [0 ] * image_sz [0 ] / (math .tan (hfov / 2 )* distance * 2 ))
110+ box_w = ceil (obj_sz [1 ] * image_sz [1 ] / (math .tan (vfov / 2 )* distance * 2 ))
111+ return box_h , box_w
112+
113+ #convert horizonal fov to vertical fov
114+ def hfov2vfov (hfov , image_sz ):
115+ aspect = image_sz [0 ]/ image_sz [1 ]
116+ vfov = 2 * math .atan ( tan (hfov / 2 ) * aspect )
117+ return vfov
118+
119+ #matrix with all ones
120+ def equal_weight_mtx (roi_h ,roi_w ):
121+ return np .ones ((roi_h ,roi_w ))
122+
123+ #matrix with max weight in center and decreasing linearly with distance from center
124+ def linear_weight_mtx (roi_h ,roi_w ):
125+ w_mtx = np .ones ((roi_h ,roi_w ))
126+ for j in range (0 ,roi_w ):
127+ for i in range (j ,roi_h - j ):
128+ w_mtx [j :roi_h - j ,i :roi_w - i ] = (j + 1 )
129+ return w_mtx
130+
131+ #matrix with max weight in center and decreasing quadratically with distance from center
132+ def square_weight_mtx (roi_h ,roi_w ):
133+ w_mtx = np .ones ((roi_h ,roi_w ))
134+ for j in range (0 ,roi_w ):
135+ for i in range (j ,roi_h - j ):
136+ w_mtx [j :roi_h - j ,i :roi_w - i ] = (j + 1 )* (j + 1 )
137+ return w_mtx
138+
139+ def print_stats (img ):
140+ projectairsim_log ().info ('Avg: ' ,np .average (img ))
141+ projectairsim_log ().info ('Min: ' ,np .min (img ))
142+ projectairsim_log ().info ('Max: ' ,np .max (img ))
143+ projectairsim_log ().info ('Img Sz: ' ,np .size (img ))
144+
145+ def generate_depth_viz (img ,thres = 0 ):
146+ if thres > 0 :
147+ img [img > thres ] = thres
148+ else :
149+ img = np .reciprocal (img )
150+ return img
151+
152+ def moveUAV (client ,pos ,yaw ):
153+ drone .set_pose (Pose ({"translation" :Vector3 ({"x" :pos [0 ], "y" :pos [1 ], "z" :pos [2 ]}), "rotation" :to_quaternion (0 , 0 , yaw )}), True )
154+
155+
156+ pp = pprint .PrettyPrinter (indent = 4 )
157+
158+ client = ProjectAirSimClient ()
159+ client .connect ()
160+
161+ try :
162+ world = World (client , "scene_computer_vision.jsonc" , delay_after_load_sec = 2 )
163+ drone = Drone (client , world , "Drone1" )
164+
165+ #Define start position, goal and size of UAV
166+ pos = [0 ,5 ,- 3 ] #start position x,y,z
167+ goal = [160 ,0 ] #x,y
168+ uav_size = [0.29 * 3 ,0.98 * 2 ] #height:0.29 x width:0.98 - allow some tolerance
169+
170+ #Define parameters and thresholds
171+ hfov = radians (90 )
172+ coll_thres = 5000
173+ yaw = 0
174+ limit_yaw = 5
175+ step = 0.1
176+
177+ responses = drone .get_images ("front_right" , [ImageType .DEPTH_PLANAR ])
178+ response = responses [ImageType .DEPTH_PLANAR ]
179+
180+ #initial position
181+ moveUAV (client ,pos ,yaw )
182+
183+ predictControl = AvoidLeft (hfov , coll_thres , yaw , limit_yaw , step )
184+
185+ for z in range (10000 ): # do few times
186+
187+ # get response
188+ responses = drone .get_images ("front_right" , [ImageType .DEPTH_PLANAR ])
189+ response = responses [ImageType .DEPTH_PLANAR ]
190+
191+ # get numpy array
192+ img2d = unpack_image (response )
193+
194+ [pos ,yaw ,target_dist ] = predictControl .get_next_vec (img2d , uav_size , goal , pos )
195+ moveUAV (client ,pos ,yaw )
196+
197+ if (target_dist < 1 ):
198+ projectairsim_log ().info ('Target reached.' )
199+ projectairsim_log ().info ('Press enter to continue' )
200+ input ()
201+ break
202+
203+ drone .set_pose (Pose ({"translation" :Vector3 ({"x" :0 , "y" :0 , "z" :0 }), "rotation" :to_quaternion (0 , 0 , 0 )}), True )
204+ finally :
205+ client .disconnect ()
0 commit comments