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