class Execute_Class: def __init__(self): self.pose = { 'p_x': 0, 'p_y': 0, 'p_z': 0, 'r_x': 0, 'r_y': 0, 'r_z': 0 } self.pred_r = 0 # r parameter self.optimal_path = None self.Duration = 1.5 self.time_interval = 0.02 self.time_interval_execute = 0.02 self.update_path = False self.con = Commander() self.path_queue_size = 60 self.goal_pose = np.zeros(4) self.mav_pose = np.zeros(6) self.path_queue = myQueue(self.path_queue_size) self.path_buff = myQueue(self.path_queue_size) self.refresh_buff_flag = False # rospy.init_node("execute_path_node") rate = rospy.Rate(100) self.path_generation_pub = rospy.Publisher('our/path/generation', Vector3, queue_size=10) self.movement_pub = rospy.Publisher('our/path/movement', Vector3, queue_size=10) self.pred_pose_sub = rospy.Subscriber( "our/gate_pose_pred/pose_for_path", PoseStamped, self.pred_pose_callback) # Define the input limits: fmin = 1 #[m/s**2] fmax = 100 #[m/s**2] wmax = 100 #[rad/s] minTimeSec = 0.02 #[s] # Define how gravity lies: gravity = [0, 0, -9.81] self.path_handle = Generate_Path(fmin, fmax, wmax, minTimeSec, gravity) ''' quater to euler angle /degree ''' def quater_to_euler(self, q): w = q[0] x = q[1] y = q[2] z = q[3] phi = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) theta = math.asin(2 * (w * y - z * x)) psi = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y)) Euler_Roll_x = phi * 180 / math.pi Euler_Pitch_y = theta * 180 / math.pi Euler_Yaw_z = psi * 180 / math.pi return (Euler_Roll_x, Euler_Pitch_y, Euler_Yaw_z) def yaw_sigmoid_func(self, x): eps = 10e-5 x = x + 5 x = x s = np.log(x) return s def update_path_func(self): global lock cur_pos = self.con.get_current_mav_pose() dict_pos = {} dict_pos['Pos_x'] = cur_pos.pose.position.x dict_pos['Pos_y'] = cur_pos.pose.position.y dict_pos['Pos_z'] = cur_pos.pose.position.z dict_pos['Quaternion_x'] = cur_pos.pose.orientation.x dict_pos['Quaternion_y'] = cur_pos.pose.orientation.y dict_pos['Quaternion_z'] = cur_pos.pose.orientation.z dict_pos['Quaternion_w'] = cur_pos.pose.orientation.w q = np.array([ dict_pos['Quaternion_w'], dict_pos['Quaternion_x'], dict_pos['Quaternion_y'], dict_pos['Quaternion_z'] ]) euler_angle = self.quater_to_euler(q) self.mav_pose = np.array([dict_pos['Pos_x'],dict_pos['Pos_y'],dict_pos['Pos_z'],\ euler_angle[0],euler_angle[1],euler_angle[2]],np.float) # print ('mav_pose:',mav_pose) ''' the coordinate between the path planner and gazebo is different ''' pos0 = [self.mav_pose[0], self.mav_pose[1], self.mav_pose[2]] #position vel0 = self.path_handle.generate_vel_from_yaw(self.mav_pose[5]) acc0 = [0, 0, 0] #acceleration self.goal_pose[0] = self.mav_pose[0] + self.pose['p_x'] self.goal_pose[1] = self.mav_pose[1] + self.pose['p_y'] self.goal_pose[2] = np.clip(self.mav_pose[2] + self.pose['p_z'], 1.2, 1.5) self.goal_pose[3] = self.mav_pose[5] + self.pose['r_z'] self.goal_pose[3] = -360 + self.goal_pose[3] if self.goal_pose[ 3] > 180 else self.goal_pose[3] self.goal_pose[3] = 360 + self.goal_pose[3] if self.goal_pose[ 3] < -180 else self.goal_pose[3] posf = [self.goal_pose[0], self.goal_pose[1], self.goal_pose[2]] # position velf = self.path_handle.generate_vel_from_yaw( self.goal_pose[3]) # velocity accf = [0, 0, 0] # acceleration #self.Duration = self.pred_r*1.1 self.optimal_path = self.path_handle.get_paths_list( pos0, vel0, acc0, posf, velf, accf, self.Duration) print('~~~~~~~~~~~~~*************~~~~~~~~~~~~~~') print('jump_goal:', self.goal_pose) next_x = 0 next_y = 0 next_z = 0 theta = 0 start_t = (self.Duration - self.path_queue_size * self.time_interval_execute) / 1.2 stop_t = start_t + self.path_queue_size * self.time_interval_execute # yaw_delta = self.pose['r_z']/self.path_queue_size # print ("self.pose['r_z']",self.pose['r_z']) yaw_a = self.pose['r_z'] / ( self.yaw_sigmoid_func(stop_t - self.time_interval_execute) - self.yaw_sigmoid_func(start_t)) #[),so stop_t - time_interval yaw_b = self.mav_pose[5] - yaw_a * self.yaw_sigmoid_func(start_t) theta = self.mav_pose[5] for t in np.arange(start_t, stop_t, self.time_interval_execute): pass ''' break down the present path and use the replanning path ''' if (self.path_buff.isFull() == True): break next_x = np.float(self.optimal_path.get_position(t)[0]) next_y = np.float(self.optimal_path.get_position(t)[1]) next_z = 1.2 #np.float(self.optimal_path.get_position(t)[2]) theta = yaw_a * self.yaw_sigmoid_func(t) + yaw_b # # theta = theta + yaw_delta ''' deal with the point of -180->180 ''' theta = -360 + theta if theta > 180 else theta theta = 360 + theta if theta < -180 else theta lock.acquire() self.path_buff.add(np.array([next_x, next_y, next_z, theta])) lock.release() def generate_path(self): while (1): if (self.refresh_buff_flag == True): self.refresh_buff_flag = False self.path_queue.clear() self.path_buff.clear() self.update_path_func() for i in self.path_buff.queue(): self.path_queue.add(i) else: self.path_buff.clear() self.update_path_func() #self.path_queue.show() def pred_pose_callback(self, msg): self.pose['p_x'] = msg.pose.position.x self.pose['p_y'] = msg.pose.position.y self.pose['p_z'] = msg.pose.position.z self.pose['r_x'] = msg.pose.orientation.x self.pose['r_y'] = msg.pose.orientation.y self.pose['r_z'] = msg.pose.orientation.z self.pred_r = np.sqrt( pow(self.pose['p_x'], 2) + pow(self.pose['p_y'], 2) + pow(self.pose['p_z'], 2)) self.update_path = True ##start the generate thread #print ('raw_pose',self.pose) def pass_through(self): global lock if self.optimal_path is not None: while (self.path_queue.isEmpty() == False): if (self.path_queue.isEmpty() == False): path_tmp = self.path_queue.get() # print ('next_piont:',path_tmp) # print ('~~~~~~~~~~~~~*************~~~~~~~~~~~~~~') self.con.move(path_tmp[0], path_tmp[1], path_tmp[2], path_tmp[3], False) time.sleep(0.02) self.refresh_buff_flag = True def run(self): global jump_once ''' execute the generated path ''' if (self.update_path): self.pass_through()
if __name__ == "__main__": con = Commander() time.sleep(1) target = PoseStamped() #THIS IS WHERE THE TARGET IS SET #CHANGE ACCORDING TO WHERE THE DRONE IS BEING TESTED #CPS BUILDING INDOORS <2m target.pose.position.x = 10 target.pose.position.y = 0 target.pose.position.z = 1 while True: if current_position.pose.position.x <= target.pose.position.x - 0.2: print('moving x') if obstacle == 0: con.move(2, 0, 0) time.sleep(2) elif obstacle == 1: con.move(0, 3, 0) time.sleep(2) elif obstacle == 2: con.move(-1, 3, 0) time.sleep(2) else: con.move(-0.5, 0, 0) time.sleep(2) if current_position.pose.position.y <= target.pose.position.y - 0.2: print('moving y') con.move(0, 1, 0) time.sleep(2)
from commander import Commander import time con = Commander() time.sleep(2) # use FLU body frame print "moving 3 meter to the right" con.move(0, -3, 0) time.sleep(8) print "turn 90 degrees anticlockwise" con.turn(90) time.sleep(10) print "moving 3 meter to the right" con.move(0, -3, 0) time.sleep(8) print "turn 90 degrees anticlockwise" con.turn(180) time.sleep(10) print "moving 3 meter to the right" con.move(0, -3, 0) time.sleep(8) print "turn 90 degrees anticlockwise" con.turn(270) time.sleep(10)
from commander import Commander import time import rospy rospy.init_node('init_drone') con = Commander() time.sleep(0.5) con.move(-5, 0, 0) time.sleep(0.5)
class SimpleTrackAndMove: def __init__( self, resolution, K, commander, left_topic='/gi/simulation/left/image_raw', right_topic='/gi/simulation/right/image_raw', object_position_topic='/track_rect_pub', move_threshold=0.3, altitude=3000, stereo=None, baseline=None, ): self.fx = float(K[0][0]) self.fy = float(K[1][1]) # self.idx = 0 self.con = Commander() time.sleep(0.1) self.left_sub = message_filters.Subscriber(left_topic, Image) self.right_sub = message_filters.Subscriber(right_topic, Image) self.object_position_sub = message_filters.Subscriber( object_position_topic, Int32MultiArray) ts = message_filters.ApproximateTimeSynchronizer( [self.left_sub, self.right_sub, self.object_position_sub], 10, 0.1, allow_headerless=True) ts.registerCallback(self.update_new_position) self.prev_position = None self.cur_position = None self.width = resolution[0] self.hight = resolution[1] self.camera_center = self.get_center((0, 0, self.width, self.hight)) self.move_threshold = move_threshold self.altitude = altitude self.stereo = stereo if stereo is not None: assert baseline is not None self.baseline = baseline self.bridge = CvBridge() rospy.spin() def check_border(self, box): x1 = max(box[0], 0) if box[0] <= self.width else self.width y1 = max(box[1], 0) if box[1] <= self.hight else self.hight x2 = max(box[2], 0) if box[2] <= self.width else self.width y2 = max(box[3], 0) if box[3] <= self.hight else self.hight return (x1, y1, x2, y2) def get_center(self, box): x1 = box[0] y1 = box[1] x2 = box[2] y2 = box[3] return (abs(x2 + x1) / 2., abs(y2 + y1) / 2.) def update_new_position(self, left, right, track_result): left_img = self.bridge.imgmsg_to_cv2(left, "mono8") right_img = self.bridge.imgmsg_to_cv2(right, "mono8") box = track_result.data self.cur_position = self.check_border(box) # if self.idx%5 == 0: # left_img_rgb = self.bridge.imgmsg_to_cv2(left, "bgr8") # left_img_rgb = cv2.rectangle(left_img_rgb, (self.cur_position[0],self.cur_position[1]), (self.cur_position[2],self.cur_position[3]), (255,0,0)) # cv2.imwrite('/home/gi/Documents/img/{}.png'.format(str(self.idx)), left_img_rgb) # self.idx+=1 self.update_altitude(left_img, left_img, track_result) new_move = self.get_next_move() if new_move is not None: print('Move:', new_move) self.con.move(new_move[0], new_move[1], new_move[2]) time.sleep(0.1) def update_altitude(self, left, right, track_result): if self.stereo is not None: disparity = stereo.compute(imgL, imgR) disparity = cv2.convertScaleAbs(disparity, disparity, alpha=1. / 16) mean_disparity = np.mean( disparity[track_result[1]:track_result[3], track_result[0]:track_result[2]]) mean_depth = self.baseline * self.fx / mean_disparity self.altitude = mean_depth else: pass def get_point_dist(self, p1, p2): return np.sqrt(np.sum(np.square(np.asarray(p1) - np.asarray(p2)))) def get_dist(self, a, b): return np.sqrt(np.sum(np.square(a) + np.square(b))) def get_next_move(self): print('Current position box: ', self.cur_position) cur_center = self.get_center(self.cur_position) #|----^ X---| #|----|-----| #|Y---|-----| #<————+-----| #|----------| #|----------| y = -(cur_center[0] - self.camera_center[0]) * self.altitude / self.fx x = -(cur_center[1] - self.camera_center[1]) * self.altitude / self.fy if self.get_dist(x, y) > self.move_threshold: return (x / 1000., y / 1000., 0) else: return None
# Define the input limits: fmin = 0.1 #[m/s**2] fmax = 2 #[m/s**2] wmax = 0.79 #[rad/s] minTimeSec = 0.02 #[s] # Define how gravity lies: gravity = [0,0,-9.81] p = Generate_Path(fmin,fmax,wmax, minTimeSec,gravity) optimal_path = p.get_paths_list(pos0,vel0,acc0,posf,velf,accf,Tf) con = Commander() for i in range(10): con.move(0,0,1,0,False) time.sleep(1) time_interval = 0.01 next_x = 0 next_y = 0 next_z = 1 theta = 0 plot_arr = [[] for i in range(3)] for t in np.arange(0,Tf+time_interval,time_interval): next_x = np.float(optimal_path.get_position(t)[0]) next_y = 10*np.float(optimal_path.get_position(t)[1]) next_z = np.float(optimal_path.get_position(t)[2]) theta = p.generate_yaw_from_vel(optimal_path.get_velocity(t)) print (next_x,next_y,next_z,theta) con.move(next_x,next_y,next_z,theta,False) for idx in range(3):
class Execute_Class: def __init__(self): self.pose = { 'p_x': 0, 'p_y': 0, 'p_z': 0, 'r_x': 0, 'r_y': 0, 'r_z': 0 } self.pred_r = 0 # r parameter self.optimal_path = None self.Duration = 3 self.update_path = False self.con = Commander() self.path_queue_size = 70 self.path_queue = queue.Queue(self.path_queue_size) # rospy.init_node("execute_path_node") rate = rospy.Rate(100) self.path_generation_pub = rospy.Publisher('gi/path/generation', Vector3, queue_size=10) self.movement_pub = rospy.Publisher('gi/path/movement', Vector3, queue_size=10) self.pred_pose_sub = rospy.Subscriber( "gi/gate_pose_pred/pose_for_path", PoseStamped, self.pred_pose_callback) # Define the input limits: fmin = 0.1 #[m/s**2] fmax = 2 #[m/s**2] wmax = 0.79 #[rad/s] minTimeSec = 0.02 #[s] # Define how gravity lies: gravity = [0, 0, -9.81] self.path_handle = Generate_Path(fmin, fmax, wmax, minTimeSec, gravity) ''' quater to euler angle /degree ''' def quater_to_euler(self, q): w = q[0] x = q[1] y = q[2] z = q[3] phi = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) theta = math.asin(2 * (w * y - z * x)) psi = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y)) Euler_Roll_x = phi * 180 / math.pi Euler_Pitch_y = theta * 180 / math.pi Euler_Yaw_z = psi * 180 / math.pi return (Euler_Roll_x, Euler_Pitch_y, Euler_Yaw_z) def generate_path(self): pos = self.con.get_current_mav_pose() dict_pos = {} dict_pos['Pos_x'] = pos.pose.position.x dict_pos['Pos_y'] = pos.pose.position.y dict_pos['Pos_z'] = pos.pose.position.z dict_pos['Quaternion_x'] = pos.pose.orientation.x dict_pos['Quaternion_y'] = pos.pose.orientation.y dict_pos['Quaternion_z'] = pos.pose.orientation.z dict_pos['Quaternion_w'] = pos.pose.orientation.w q = np.array([ dict_pos['Quaternion_w'], dict_pos['Quaternion_x'], dict_pos['Quaternion_y'], dict_pos['Quaternion_z'] ]) euler_angle = self.quater_to_euler(q) mav_pose = np.array([dict_pos['Pos_x'],dict_pos['Pos_y'],dict_pos['Pos_z'],\ euler_angle[0],euler_angle[1],euler_angle[2]],np.float) # print ('mav_pose:',mav_pose) ''' the coordinate between the path planner and gazebo is different ''' pos0 = [mav_pose[0], mav_pose[1], mav_pose[2]] #position vel0 = [ -np.cos(np.deg2rad(mav_pose[3])), np.sin(np.deg2rad(mav_pose[3])), 0 ] #velocity acc0 = [0, 0, 0] #acceleration goal_pose = np.zeros(4) goal_pose[0] = mav_pose[0] + self.pose['p_x'] goal_pose[1] = mav_pose[1] + self.pose['p_y'] goal_pose[2] = np.clip(mav_pose[2] + self.pose['p_z'], 1.9, 2.5) goal_pose[3] = mav_pose[3] + self.pose['r_z'] posf = [goal_pose[0], goal_pose[1], goal_pose[2]] # position velf = [ -np.cos(np.deg2rad(goal_pose[3])), np.sin(np.deg2rad(goal_pose[3])), 0 ] # velocity accf = [0, 0, 0] # acceleration #self.Duration = self.pred_r*1.1 self.optimal_path = self.path_handle.get_paths_list( pos0, vel0, acc0, posf, velf, accf, self.Duration) #print ('~~~~~~~~~~~~~*************~~~~~~~~~~~~~~') #print ('jump_goal:',goal_pose) #return self.optimal_path def pred_pose_callback(self, msg): self.pose['p_x'] = msg.pose.position.x self.pose['p_y'] = msg.pose.position.y self.pose['p_z'] = msg.pose.position.z self.pose['r_x'] = msg.pose.orientation.x self.pose['r_y'] = msg.pose.orientation.y self.pose['r_z'] = msg.pose.orientation.z self.pred_r = np.sqrt( pow(self.pose['p_x'], 2) + pow(self.pose['p_y'], 2) + pow(self.pose['p_z'], 2)) self.update_path = True self.generate_path() #print ('raw_pose',self.pose) def pass_through(self): if self.optimal_path is not None: time_interval = 0.02 next_x = 0 next_y = 0 next_z = 0 theta = 0 print('update_path:', self.update_path) print('~~~~~~~~~~~~~*************~~~~~~~~~~~~~~') print(self.path_queue) while self.path_queue.empty() == False: path_tmp = self.path_queue.get() self.con.move(path_tmp[0], path_tmp[1], path_tmp[2], path_tmp[3], False) time.sleep(0.05) for t in np.arange( self.Duration - self.path_queue_size * time_interval, self.Duration, time_interval): ''' break down the present path and use the replanning path ''' if self.path_queue.full() == True: break next_x = np.float(self.optimal_path.get_position(t)[0]) next_y = np.float(self.optimal_path.get_position(t)[1]) next_z = 1.93 #np.float(self.optimal_path.get_position(t)[2]) self.path_generation_pub.publish( Vector3(next_x, next_y, next_z)) pos = self.con.get_current_mav_pose() q = np.array([ pos.pose.orientation.w, pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z ]) euler_angle = self.quater_to_euler(q) theta = self.path_handle.generate_yaw_from_vel( self.optimal_path.get_velocity(t), euler_angle[2]) self.path_queue.put(np.array([next_x, next_y, next_z, theta])) # self.con.move(next_x,next_y,next_z,theta,False) #time.sleep(0.02) return np.array([next_x, next_y, next_z, theta]) def run(self): global jump_once ''' execute the generated path ''' if (jump_once > 0): now_point = self.pass_through() #jump_once = jump_once -1 time.sleep(0.1)
from commander import Commander import time con = Commander() time.sleep(2) # turn 90 anti-clockwise print("turn to 90 deg!") con.turn(90) time.sleep(3) # move left relative to BODY_OFFSET_NED frame print("move 20m to the left!") con.move(20, 0, 0) time.sleep(20) # turn 90 anti-clockwise print("turn to 180 deg!") con.turn(180) time.sleep(3) # move left relative to BODY_OFFSET_NED frame print("move 15m to the left!") con.move(15, 0, 0) time.sleep(15) # turn 90 anti-clockwise print("turn to 270 deg!") con.turn(270) time.sleep(3)