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()
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)
