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)
