def sendControl(event):
global getState
global setState
global flag
global roll
global pitch
global gaz
global yaw
global airborne
if flag == 4:
reset = SetModelState()
reset.pose = Pose()
reset.pose.position.x = reset.pose.position.y = reset.pose.position.z = .04
reset.pose.orientation.x = reset.pose.orientation.y = reset.pose.orientation.z = 0
reset.pose.orientation.w = 1
reset.twist = Twist()
reset.model_name = droneName
reset.reference_frame = "world"
airborne = False
setState(reset)
else:
if flag == 3 and not airborne:
takeoff()
airborne = True
elif flag == 2 and airborne:
land()
airborne = False
elif (flag == 0 or flag == 1) and airborne:
movement(event.last_duration)
def takeoff():
currState = getState(droneName,"world")
oldz = currState.pose.position.z
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
new.twist.linear.z = .4
setState(new)
while(currState.pose.position.z < 1.0 + oldz):
currState = getState(droneName,"world")
rospy.sleep(.01)
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
setState(new)
def land():
currState = getState(droneName,"world")
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
new.twist.linear.z = -.4
setState(new)
rospy.sleep(.05)
currState = getState(droneName,"world")
while(abs(currState.twist.linear.z) > .02):
currState = getState(droneName,"world")
rospy.sleep(.01)
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
setState(new)
def sendControl(event):
global getState
global setState
global flag
global roll
global pitch
global gaz
global yaw
global airborne
if flag == 4:
reset = SetModelState()
reset.pose = Pose()
reset.pose.position.x = reset.pose.position.y = reset.pose.position.z = .04
reset.pose.orientation.x = reset.pose.orientation.y = reset.pose.orientation.z = 0
reset.pose.orientation.w = 1
reset.twist = Twist()
reset.model_name = droneName
reset.reference_frame = "world"
airborne = False
setState(reset)
else:
if flag == 3 and not airborne:
takeoff()
airborne = True
elif flag == 2 and airborne:
land()
airborne = False
elif (flag == 0 or flag == 1) and airborne:
movement(event.last_duration)
def movement(dt): global getState global setState global flag global roll global pitch global gaz global yaw global airborne curr = getState(droneName,"world") new = SetModelState() new.model_name = droneName new.reference_frame = "world" new.pose = curr.pose # add random movements :) new.twist = Twist() r,p,currYaw = tf.transformations.euler_from_quaternion([new.pose.orientation.x,new.pose.orientation.y,new.pose.orientation.z,new.pose.orientation.w]) x = -pitch y = roll globalx = x * cos(currYaw) + y * sin(currYaw) globaly = x * sin(currYaw) - y * cos(currYaw) new.twist.linear.x = accel(globalx,curr.twist.linear.x,dt) new.twist.linear.y = accel(globaly,curr.twist.linear.y,dt) new.twist.linear.z = accel(gaz,curr.twist.linear.z,dt) new.twist.angular.z = accel(-yaw,curr.twist.angular.z,dt) new.twist.linear.x += (random.random() - .5)/turbMult new.twist.linear.y += (random.random() - .5)/turbMult new.twist.linear.z += (random.random() - .5)/turbMult new.twist.angular.x += (random.random() - .5)/turbMult new.twist.angular.y += (random.random() - .5)/turbMult new.twist.angular.z += (random.random() - .5)/turbMult nqo = calcRP(new) new.pose.orientation.x = nqo[0] new.pose.orientation.y = nqo[1] new.pose.orientation.z = nqo[2] new.pose.orientation.w = nqo[3] setState(new) rospy.sleep(.01)
def movement(dt):
global getState
global setState
global flag
global roll
global pitch
global gaz
global yaw
global airborne
curr = getState(droneName, "world")
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = curr.pose
# add random movements :)
new.twist = Twist()
r, p, currYaw = tf.transformations.euler_from_quaternion([
new.pose.orientation.x, new.pose.orientation.y, new.pose.orientation.z,
new.pose.orientation.w
])
x = -pitch
y = roll
globalx = x * cos(currYaw) + y * sin(currYaw)
globaly = x * sin(currYaw) - y * cos(currYaw)
new.twist.linear.x = accel(globalx, curr.twist.linear.x, dt)
new.twist.linear.y = accel(globaly, curr.twist.linear.y, dt)
new.twist.linear.z = accel(gaz, curr.twist.linear.z, dt)
new.twist.angular.z = accel(-yaw, curr.twist.angular.z, dt)
new.twist.linear.x += (random.random() - .5) / turbMult
new.twist.linear.y += (random.random() - .5) / turbMult
new.twist.linear.z += (random.random() - .5) / turbMult
new.twist.angular.x += (random.random() - .5) / turbMult
new.twist.angular.y += (random.random() - .5) / turbMult
new.twist.angular.z += (random.random() - .5) / turbMult
nqo = calcRP(new)
new.pose.orientation.x = nqo[0]
new.pose.orientation.y = nqo[1]
new.pose.orientation.z = nqo[2]
new.pose.orientation.w = nqo[3]
setState(new)
rospy.sleep(.01)
def takeoff():
currState = getState(droneName, "world")
oldz = currState.pose.position.z
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
new.twist.linear.z = .4
setState(new)
while (currState.pose.position.z < 1.0 + oldz):
currState = getState(droneName, "world")
rospy.sleep(.01)
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
setState(new)
def land():
currState = getState(droneName, "world")
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
new.twist.linear.z = -.4
setState(new)
rospy.sleep(.05)
currState = getState(droneName, "world")
while (abs(currState.twist.linear.z) > .02):
currState = getState(droneName, "world")
rospy.sleep(.01)
new = SetModelState()
new.model_name = droneName
new.reference_frame = "world"
new.pose = currState.pose
new.twist = Twist()
setState(new)
