def _reset_fired(self):
cmd = trajectory()
cmd.xc_2dot = [0, 0, 0]
cmd.xc_dot = [0, 0, 0]
cmd.xc = [0, 0, 0]
self.xd = cmd.xc
cmd.b1 = [1, 0, 0]
pub.publish(cmd)
def __init__(self):
rospy.init_node('trajectory_echo', anonymous=True)
rospy.Subscriber(desiredPoseTopic, PoseStamped, self.callback_pose)
rospy.Subscriber(desiredTwistTopic, TwistStamped, self.callback_twist)
rospy.Subscriber(desiredAccelTopic, AccelStamped, self.callback_accel)
self.pub = rospy.Publisher(desiredTrajectory, trajectory, queue_size= 1)
self.cmd = trajectory()
self.cmd.b1 = [1, 0, 0]
self.OmegaZ = 0
self.getsPose = False
self.getsVel = False
self.getsAcc = False
class CmdThread(Thread, HasTraits):
"""
Trajectory command thread
Parameters
----------
Missions:
take off: landing at current (x, y) position
land: landing at current (x, y) position
spin: spin starting at (0, 0)
halt: positins hold at last Xd
"""
wants_abort = False
cmd = trajectory()
cmd.b1 = [1, 0, 0]
cmd.xc = [0, 0, 0]
mission = 'halt'
t_init = time.time()
t_cur = 0
name = uav_name
x_v = Array(shape=(3, ))
x_euler = Array(shape=(3, ))
br = tf.TransformBroadcaster()
yaw = 0
def __init__(self, args):
Thread.__init__(self)
self.args = args
self.theta = 0
self.motor_set(True, True)
def motor_set(self, motor, warmup):
rospy.set_param('/' + self.name + '/uav/Motor', motor)
rospy.set_param('/' + self.name + '/uav/MotorWarmup', warmup)
def cmd_tf_pub(self):
self.br.sendTransform(
self.cmd.xc,
tf.transformations.quaternion_from_euler(0, 0, self.x_euler[2]),
rospy.Time.now(), self.name + '_xc', "world")
def run(self):
print(self.args)
print('Process: cmd thread started')
self.cmd.header.frame_id = uav_name + '/uav'
dt = 0.1
z_min = 0.2
v_up = 0.3
t_total = 5
z_hover = 1.0
z_land = self.x_v[2]
cmd = self.cmd
cmd.xc = [0, 0, 0]
cmd.xc_dot = [0, 0, 0]
cmd.xc_2dot = [0, 0, 0]
cmd.b1 = [1, 0, 0]
x_v = self.x_v
x_euler = self.x_euler
self.motor_set(True, True)
motor_flag = False
pub.publish(self.cmd)
self.cmd_tf_pub()
while not self.wants_abort:
print('Mission: {}, time: {:2.4f} sec'.format(
self.mission, self.t_cur))
self.t_cur = time.time() - self.t_init
cmd.header.stamp = rospy.get_rostime()
if self.mission == 'take off':
self.motor_set(True, False)
motor_flag = True
cmd.b1 = [np.cos(x_euler[2]), np.sin(x_euler[2]), 0]
if self.t_cur <= t_total and self.mission == 'take off':
height = z_min + v_up * self.t_cur
cmd.xc = [
x_v[0], x_v[1], height if height < z_hover else z_hover
]
cmd.xc_dot = [0, 0, v_up * dt]
if z_hover == height:
continue
self.xd = cmd.xc
else:
self.mission = 'halt'
cmd.xc_dot = [0, 0, 0]
pub.publish(self.cmd)
# print('Take off complete')
elif self.mission == 'spin':
# TODO
t_total = 30
if self.t_cur <= t_total:
self.theta = 2 * np.pi / t_total * self.t_cur
cmd.b1 = [
np.cos(x_euler[2] + self.theta),
np.sin(x_euler[2] + self.theta), 0
]
cmd.xc = [x_v[0], x_v[1], z_hover]
# cmd.xc = [(np.cos(theta)-1.)/2.0, 1./2.0*np.sin(theta),z_hover]
# cmd.xc_dot = [dt*np.sin(theta)/2.0, dt*1./2.0*np.sin(theta),0]
if x_v[2] < z_min:
rospy.set_param('/Jetson/uav/Motor', False)
print(cmd.xc)
else:
self.mission = 'halt'
pub.publish(self.cmd)
self.theta = 0
elif self.mission == 'land':
rospy.set_param(explore_flag, False)
t_total = z_land / v_up
cmd.b1 = [np.cos(x_euler[2]), np.sin(x_euler[2]), 0]
if self.t_cur <= t_total and self.mission == 'land':
height = z_land - (v_up * self.t_cur)
cmd.xc[0], cmd.xc[1] = x_v[0], x_v[1]
cmd.xc[2] = height if height > z_min else z_min
# cmd.xc = [x_v[0],x_v[1],height if height > z_min else z_min]
cmd.xc_dot = [0, 0, 0]
if z_min == height:
continue
self.xd = cmd.xc
else:
self.motor_set(False, False)
self.wants_abort = True
pub.publish(self.cmd)
# print(cmd.xc)
# pub.publish(cmd)
elif self.mission == 'halt':
# TODO
# if x_v[2] < z_min:
# rospy.set_param('/Jetson/uav/Motor', False)
x_v = self.x_v
x_euler = self.x_euler
cmd.xc[2] = z_hover
cmd.xc_dot = [0, 0, 0]
self.t_cur = 0
self.t_init = time.time()
z_land = self.x_v[2]
if not motor_flag:
cmd.xc = self.x_v
cmd.xc_dot = [0, 0, 0]
self.xd = cmd.xc
if self.mission != 'halt':
pub.publish(self.cmd)
self.cmd_tf_pub()
time.sleep(dt)
pass
self.motor_set(False, False)
pub.publish(self.cmd)
print('Process: cmd thread killed')
def _xd_changed(self):
cmd = trajectory()
cmd.xc_2dot = [0, 0, 0]
cmd.xc_dot = [0, 0, 0]
cmd.xc = self.xd
cmd.b1 = [1, 0, 0]
def mission_request():
global x_v
get_key()
dt = 0.01
t_init = time.time()
cmd = trajectory()
cmd.b1 = [1,0,0]
cmd.header.frame_id = 'uav'
t_total = mission['t_mission']
t_cur = 0
#print('motor: '+str(rospy.get_param('/'+uav_name+'/uav')))
#print('motor warmup: '+str(rospy.get_param('/'+uav_name+'/uavWarmup')))
print(mission['mode'])
if mission['mode'] == 'kill':
motor_set(False,False)
print('Stopping motors')
time.sleep(.1)
elif mission['mode'] == 'warmup':
motor_set(True,True)
time.sleep(2)
pub.publish(cmd)
mission['warmup'] = False
elif mission['mode'] == 'reset':
rospy.set_param('/'+uav_name+'/uav/MotorWarmup', True)
print('Motor warmup OFF')
rospy.set_param('/'+uav_name+'/uav/MotorWarmup', False)
print('Motor warmup OFF')
pub.publish(cmd)
elif mission['mode'] == 'quit':
print('terminating mission')
rospy.set_param('/'+uav_name+'/uav', False)
rospy.set_param('/'+uav_name+'/uav/MotorWarmup', False)
sys.exit()
elif mission['mode'] == 'take off':
print('Motor warmup ON')
motor_set(True,True)
rospy.sleep(2)
motor_set(True,False)
print('Taking off at {} sec'.format(time.time()-t_init))
t_init = time.time()
while t_cur <= t_total and mission['mode'] == 'take off':
t_cur = time.time() - t_init
time.sleep(dt)
cmd.header.stamp = rospy.get_rostime()
height = z_min+v_up*t_cur
cmd.xc = [x_v[0],x_v[1],height if height < 1.5 else 1.5 ]
print(cmd.xc)
cmd.xc_dot = [0,0,v_up]
cmd_tf_pub(cmd.xc)
pub.publish(cmd)
get_key()
mission['mode'] = 'wait'
print('Take off complete')
elif mission['mode'] == 'land':
print('Landing')
z_hover = x_v[2]
while t_cur <= t_total and mission['mode'] == 'land':
t_cur = time.time() - t_init
time.sleep(dt)
cmd.header.stamp = rospy.get_rostime()
height = z_hover - v_up*t_cur
cmd.xc = [x_v[0],x_v[1],height if height > z_min else z_min/2]
cmd.xc_dot = [0,0,-v_up]
pub.publish(cmd)
cmd_tf_pub(cmd.xc)
print(cmd.xc)
get_key()
if x_v[2] < z_min:
motor_set(False, False)
break
mission['mode'] = 'wait'
print('landing complete')
elif mission['mode'] == 'spin':
# TODO
while t_cur <= t_total:
t_cur = time.time() - t_init
time.sleep(dt)
cmd.header.stamp = rospy.get_rostime()
theta = 2*np.pi/t_total*t_cur
cmd.b1 = [np.cos(theta),np.sin(theta),0]
cmd.xc = [0,0,1.5]
cmd.xc_dot = [0,0,0]
pub.publish(cmd)
get_key()
if x_v[2] < z_min:
rospy.set_param('/'+uav_name+'/uav', False)
mission['mode'] = 'wait'
print('spin')
pass
elif mission['mode'] == 'point to point':
# TODO
while t_cur <= t_total and mission['mode'] == 'point to point':
t_cur = time.time() - t_init
time.sleep(dt)
cmd.header.stamp = rospy.get_rostime()
theta = 2*np.pi/t_total*t_cur
#cmd.b1 = [np.cos(-np.pi/4*0),np.sin(-np.pi/4*0),0]
cmd.xc = [0,np.sin(theta)*2,1.5]
cmd.xc_dot = [0,0,0]
pub.publish(cmd)
get_key()
if x_v[2] < z_min:
rospy.set_param('/'+uav_name+'/uav', False)
mission['mode'] = 'wait'
print('Finish p2p')
pass
elif mission['mode'] == 'Est1':
print('Motor warmup ON')
motor_set(True,True)
rospy.sleep(4)
motor_set(True,False)
t_init = time.time()
# TODO
x0=np.array([0,0,0])
x1=np.array([0,0,1])
x2=np.array([1,1,1.5])
x3=np.array([0,1,1])
x4=np.array([0,0,1.5])
x5=np.array([1,1,1])
t1=4
dt2=6
dt3=5
dt4=4
dt5=3
t_total = 22
while t_cur <= t_total and mission['mode'] == 'Est1':
t_cur = time.time() - t_init
time.sleep(dt)
cmd.header.stamp = rospy.get_rostime()
cmd.xc_2dot = [0,0,0]
if t_cur<t1:
cmd.xc = x0+(x1-x0)*t_cur/t1
cmd.xc_dot = (x1-x0)/t1
elif t_cur<(t1+dt2):
cmd.xc = x1+(x2-x1)*(t_cur-t1)/dt2
cmd.xc_dot = (x2-x1)/dt2
elif t_cur<(t1+dt2+dt3):
cmd.xc = x2+(x3-x2)*(t_cur-t1-dt2)/dt3
cmd.xc_dot = (x3-x2)/dt3
elif t_cur<(t1+dt2+dt3+dt4):
cmd.xc = x3+(x4-x3)*(t_cur-t1-dt2-dt3)/dt4
cmd.xc_dot = (x4-x3)/dt4
elif t_cur<=(t1+dt2+dt3+dt4+dt5):
cmd.xc = x4+(x5-x4)*(t_cur-t1-dt2-dt3-dt4)/dt5
cmd.xc_dot = (x5-x4)/dt5
cmd_tf_pub(cmd.xc)
pub.publish(cmd)
get_key()
if x_v[2] < z_min:
rospy.set_param('/'+uav_name+'/uav', False)
mission['mode'] = 'wait'
print('Finish p2p')
pass
# elif mission['mode'] == 'Simon':
# print('Motor warmup ON')
# motor_set(True,True)
# rospy.sleep(2)
# motor_set(True,False)
# print('Simon')
# t_init = time.time()
# x0 = x_v #[0,0,0]
# dictionnary = initialisation(x0[0],x0[1],x0[2])
# while mission['mode'] == 'Simon':
# t_cur = time.time() - t_init
# time.sleep(dt)
# cmd.header.stamp = rospy.get_rostime()
# height = z_hover - v_up*t_cur
# d_pos = desired_pos(t_cur,x_v,dictionnary, x_ship)
# #cmd.b1 = d_pos[3]
# cmd.xc = d_pos[0]
# cmd.xc_dot = d_pos[1]
# cmd.xc_2dot = d_pos[2]
# pub.publish(cmd)
# get_key()
# if x_v[2] < z_min and t_cur > 5:
# motor_set(False, False)
# mission['mode'] = 'wait'
# print('Simon mission complete')
# rospy.set_param('/'+uav_name+'/uav', False)
# elif mission['mode'] == 'hover':
# mission['mode'] = 'wait'
# pass
#
else:
pass