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)
Beispiel #2
0
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)
Beispiel #3
0
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 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)
Beispiel #5
0
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 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)
Beispiel #7
0
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)