Example #1
0
def freTask4(drone):
    drone.speed = 0.1
    maxVideoDelay = 0.0
    maxControlGap = 0.0
    desiredSpeed = MAX_ALLOWED_SPEED

    try:
        drone.wait(1.0)
        drone.setVideoChannel( front=False )
        downloadOldVideo( drone, timeout=20.0 )
        drone.takeoff()
        startTime = drone.time
            while drone.time < startTime + 1.0:
                drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
                # TODO sonar/vision altitude
                poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), None) )
        magnetoOnStart = drone.magneto[:3]
        print "NAVI-ON"
        startTime = drone.time
        sx,sy,sz,sa = 0,0,0,0
        lastUpdate = None
        while drone.time < startTime + 600.0:
            altitude = desiredHeight
            if drone.altitudeData != None:
                altVision = drone.altitudeData[0]/1000.0
                altSonar = drone.altitudeData[3]/1000.0
                altitude = (altSonar+altVision)/2.0
                if abs(altSonar-altVision) > 0.5:
                    print altSonar, altVision
                    altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)

            sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
            if altitude > 2.5:
                # wind and "out of control"
                sz = max( -0.5, min( 0.5, desiredHeight - altitude ))
            sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))

            if drone.lastImageResult:
                lastUpdate = drone.time
                assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
                (frameNumber, timestamp), rects = drone.lastImageResult
                viewlog.dumpVideoFrame( frameNumber, timestamp )
                #TODO
                # keep history small
                videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
                videoDelay = drone.time - videoTime
                if videoDelay > 1.0:
                    print "!DANGER! - video delay", videoDelay
                maxVideoDelay = max( videoDelay, maxVideoDelay )
                toDel = 0
                for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
                    toDel += 1
                    if oldTime >= videoTime:
                        break
                poseHistory = poseHistory[:toDel]
                
                print "FRAME", frameNumber/15
Example #2
0
def downloadOldVideo( drone, timeout ):
    "download video if it is delayed more than MAX_VIDEO_DELAY"
    global g_processingEnabled
    restoreProcessing = g_processingEnabled
    print "Waiting for video to start ..."
    startTime = drone.time
    while drone.time < startTime + timeout:
        if drone.lastImageResult != None:
            (frameNumber, timestamp), rects = drone.lastImageResult
            videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
            videoDelay = drone.time - videoTime
            print videoDelay
            if videoDelay < MAX_ALLOWED_VIDEO_DELAY:
                print "done"
                break
            g_processingEnabled = False
        drone.update()
    g_processingEnabled = restoreProcessing
Example #3
0
def downloadOldVideo(drone, timeout):
    "download video if it is delayed more than MAX_VIDEO_DELAY"
    global g_processingEnabled
    restoreProcessing = g_processingEnabled
    print "Waiting for video to start ..."
    startTime = drone.time
    while drone.time < startTime + timeout:
        if drone.lastImageResult != None:
            (frameNumber, timestamp), rects = drone.lastImageResult
            videoTime = correctTimePeriod(timestamp / 1000., ref=drone.time)
            videoDelay = drone.time - videoTime
            print videoDelay
            if videoDelay < MAX_ALLOWED_VIDEO_DELAY:
                print "done"
                break
            g_processingEnabled = False
        drone.update()
    g_processingEnabled = restoreProcessing
Example #4
0
def competeRobotemRovne( drone, desiredHeight = 1.5 ):
  drone.speed = 0.1
  maxVideoDelay = 0.0
  maxControlGap = 0.0
  desiredSpeed = MAX_ALLOWED_SPEED
  refLine = DEFAULT_REF_LINE
  
  try:
    drone.wait(1.0)
    drone.setVideoChannel( front=True )
    downloadOldVideo( drone, timeout=20.0 )
    drone.takeoff()
    poseHistory = []
    startTime = drone.time
    while drone.time < startTime + 1.0:
      drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
      # TODO sonar/vision altitude
      poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), None) )
    magnetoOnStart = drone.magneto[:3]
    print "NAVI-ON"
    startTime = drone.time
    sx,sy,sz,sa = 0,0,0,0
    lastUpdate = None
    while drone.time < startTime + 600.0:

      altitude = desiredHeight
      if drone.altitudeData != None:
        altVision = drone.altitudeData[0]/1000.0
        altSonar = drone.altitudeData[3]/1000.0
        altitude = (altSonar+altVision)/2.0
        # TODO selection based on history? panic when min/max too far??
        if abs(altSonar-altVision) > 0.5:
          print altSonar, altVision
          altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)

      sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
      if altitude > 2.5:
        # wind and "out of control"
        sz = max( -0.5, min( 0.5, desiredHeight - altitude ))

      sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))

      if drone.lastImageResult:
        lastUpdate = drone.time
        assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
        (frameNumber, timestamp), rects = drone.lastImageResult
        viewlog.dumpVideoFrame( frameNumber, timestamp )

        # keep history small
        videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
        videoDelay = drone.time - videoTime
        if videoDelay > 1.0:
          print "!DANGER! - video delay", videoDelay
        maxVideoDelay = max( videoDelay, maxVideoDelay )
        toDel = 0
        for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
          toDel += 1
          if oldTime >= videoTime:
            break
        poseHistory = poseHistory[:toDel]

        tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
        print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
#        print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
        if rects != None and len(rects) > 0:
          # note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
          if oldAltitude == None:
            oldAltitude = altitude
          debugImg = None
          if drone.replayLog != None:
            debugFilename = "tmp_%04d.jpg" % (frameNumber/FRAMES_PER_INDEX)            
            debugImg = cv2.imread( debugFilename )
          line = chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude, 
              heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg )
          if line:
            rejected = False
            if refLine != None:
              print "%.1fdeg %.2fm" % (math.degrees(normalizeAnglePIPI(refLine.angle - line.angle)), refLine.signedDistance( line.start ))
              if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
                  abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
                refLine = line
              else:
                rejected = True
            else:
              refLine = line
            viewlog.dumpBeacon( line.start, index=3 )
            viewlog.dumpBeacon( line.end, index=3 )
            if drone.replayLog != None:
              if rejected:
                # redraw image with yellow color
                chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude, 
                    heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg, color=(0,255,255) )
              cv2.imwrite( debugFilename, debugImg )
        else:
          print rects

        desiredSpeed = MAX_ALLOWED_SPEED
        if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
          desiredSpeed = 0.0

        if drone.battery < 10:
          print "BATTERY LOW!", drone.battery

        # height debugging
        #print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )

      # error definition ... if you substract that you get desired position or angle
      # error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
      errY, errA = 0.0, 0.0
      if refLine:
        errY = refLine.signedDistance( drone.coord )
        errA = normalizeAnglePIPI( drone.heading - refLine.angle )

      # get the height first
      if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
        sx = 0.0
      # error correction
      # the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
      sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
      
      # there is no drone.va (i.e. derivative of heading) available at the moment ... 
      sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK

#      print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
      prevTime = drone.time
      drone.moveXYZA( sx, sy, sz, sa )
      maxControlGap = max( drone.time - prevTime, maxControlGap )
      poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), altitude) )
    print "NAVI-OFF", drone.time - startTime
    drone.hover(0.5)
    drone.land()
    drone.setVideoChannel( front=True )    
  except ManualControlException, e:
    print "ManualControlException"
    if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
      drone.hover(0.1)
    drone.land()
Example #5
0
def followRow(drone, desiredHeight=1.5, timeout=10.0):
    maxControlGap = 0.0
    maxVideoDelay = 0.0
    desiredSpeed = MAX_ALLOWED_SPEED
    startTime = drone.time
    sx, sy, sz, sa = 0, 0, 0, 0
    lastUpdate = None
    refLine = None
    while drone.time < startTime + timeout:
        altitude = desiredHeight
        if drone.altitudeData != None:
            altVision = drone.altitudeData[0] / 1000.0
            altSonar = drone.altitudeData[3] / 1000.0
            altitude = (altSonar + altVision) / 2.0
            # TODO selection based on history? panic when min/max too far??
            if abs(altSonar - altVision) > 0.5:
                #        print altSonar, altVision
                altitude = max(altSonar,
                               altVision)  # sonar is 0.0 sometimes (no ECHO)
        sz = max(-0.2, min(0.2, desiredHeight - altitude))
        if altitude > 2.5:
            # wind and "out of control"
            sz = max(-0.5, min(0.5, desiredHeight - altitude))

        if drone.lastImageResult:
            lastUpdate = drone.time
            assert len(drone.lastImageResult) == 2 and len(
                drone.lastImageResult[0]) == 2, drone.lastImageResult
            (frameNumber, timestamp), rowTopBottom = drone.lastImageResult
            viewlog.dumpVideoFrame(frameNumber, timestamp)
            # keep history small
            videoTime = correctTimePeriod(timestamp / 1000., ref=drone.time)
            videoDelay = drone.time - videoTime
            if videoDelay > 1.0:
                print "!DANGER! - video delay", videoDelay
            maxVideoDelay = max(videoDelay, maxVideoDelay)
            toDel = 0
            for oldTime, oldPose, oldAngles in drone.poseHistory:
                toDel += 1
                if oldTime >= videoTime:
                    break
            drone.poseHistory = drone.poseHistory[:toDel]
            tiltCompensation = Pose(desiredHeight * oldAngles[0],
                                    desiredHeight * oldAngles[1],
                                    0)  # TODO real height?
            validRow = evalRowData(rowTopBottom)
            print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
                oldAngles[0]), math.degrees(oldAngles[1])), validRow
            if validRow:
                sp = groundPose(*rowTopBottom,
                                scale=ROW_WIDTH /
                                ((validRow[0] + validRow[1]) / 2.0))
                sPose = Pose(*oldPose).add(tiltCompensation).add(sp)
                refLine = Line((sPose.x, sPose.y),
                               (sPose.x + math.cos(sPose.heading),
                                sPose.y + math.sin(sPose.heading)))

        errY, errA = 0.0, 0.0
        if refLine:
            errY = refLine.signedDistance(drone.coord)
            errA = normalizeAnglePIPI(drone.heading - refLine.angle)

        sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
        sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
        sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (desiredSpeed / 0.4)
        prevTime = drone.time
        drone.moveXYZA(sx, sy, sz, sa)
        drone.poseHistory.append(
            (drone.time, (drone.coord[0], drone.coord[1], drone.heading),
             (drone.angleFB, drone.angleLR)))
        maxControlGap = max(drone.time - prevTime, maxControlGap)
    return maxControlGap
Example #6
0
def competeRobotemRovne(drone, desiredHeight=1.5):
    drone.speed = 0.1
    maxVideoDelay = 0.0
    maxControlGap = 0.0
    desiredSpeed = MAX_ALLOWED_SPEED
    refLine = DEFAULT_REF_LINE

    try:
        drone.wait(1.0)
        drone.setVideoChannel(front=True)
        downloadOldVideo(drone, timeout=20.0)
        drone.takeoff()
        poseHistory = []
        startTime = drone.time
        while drone.time < startTime + 1.0:
            drone.update("AT*PCMD=%i,0,0,0,0,0\r")  # drone.hover(1.0)
            # TODO sonar/vision altitude
            poseHistory.append(
                (drone.time, (drone.coord[0], drone.coord[1], drone.heading),
                 (drone.angleFB, drone.angleLR), None))
        magnetoOnStart = drone.magneto[:3]
        print "NAVI-ON"
        startTime = drone.time
        sx, sy, sz, sa = 0, 0, 0, 0
        lastUpdate = None
        while drone.time < startTime + 600.0:

            altitude = desiredHeight
            if drone.altitudeData != None:
                altVision = drone.altitudeData[0] / 1000.0
                altSonar = drone.altitudeData[3] / 1000.0
                altitude = (altSonar + altVision) / 2.0
                # TODO selection based on history? panic when min/max too far??
                if abs(altSonar - altVision) > 0.5:
                    print altSonar, altVision
                    altitude = max(
                        altSonar,
                        altVision)  # sonar is 0.0 sometimes (no ECHO)

            sz = max(-0.2, min(0.2, desiredHeight - altitude))
            if altitude > 2.5:
                # wind and "out of control"
                sz = max(-0.5, min(0.5, desiredHeight - altitude))

            sx = max(0, min(drone.speed, desiredSpeed - drone.vx))

            if drone.lastImageResult:
                lastUpdate = drone.time
                assert len(drone.lastImageResult) == 2 and len(
                    drone.lastImageResult[0]) == 2, drone.lastImageResult
                (frameNumber, timestamp), rects = drone.lastImageResult
                viewlog.dumpVideoFrame(frameNumber, timestamp)

                # keep history small
                videoTime = correctTimePeriod(timestamp / 1000.,
                                              ref=drone.time)
                videoDelay = drone.time - videoTime
                if videoDelay > 1.0:
                    print "!DANGER! - video delay", videoDelay
                maxVideoDelay = max(videoDelay, maxVideoDelay)
                toDel = 0
                for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
                    toDel += 1
                    if oldTime >= videoTime:
                        break
                poseHistory = poseHistory[:toDel]

                tiltCompensation = Pose(desiredHeight * oldAngles[0],
                                        desiredHeight * oldAngles[1],
                                        0)  # TODO real height?
                print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
                    oldAngles[0]), math.degrees(oldAngles[1])),
                #        print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
                if rects != None and len(rects) > 0:
                    # note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
                    if oldAltitude == None:
                        oldAltitude = altitude
                    debugImg = None
                    if drone.replayLog != None:
                        debugFilename = "tmp_%04d.jpg" % (frameNumber /
                                                          FRAMES_PER_INDEX)
                        debugImg = cv2.imread(debugFilename)
                    line = chooseBestWidth(rects,
                                           coord=oldPose[:2],
                                           height=oldAltitude,
                                           heading=oldPose[2],
                                           angleFB=oldAngles[0],
                                           angleLR=oldAngles[1],
                                           debugImg=debugImg)
                    if line:
                        rejected = False
                        if refLine != None:
                            print "%.1fdeg %.2fm" % (math.degrees(
                                normalizeAnglePIPI(refLine.angle - line.angle)
                            ), refLine.signedDistance(line.start))
                            if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
                                abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
                                refLine = line
                            else:
                                rejected = True
                        else:
                            refLine = line
                        viewlog.dumpBeacon(line.start, index=3)
                        viewlog.dumpBeacon(line.end, index=3)
                        if drone.replayLog != None:
                            if rejected:
                                # redraw image with yellow color
                                chooseBestWidth(rects,
                                                coord=oldPose[:2],
                                                height=oldAltitude,
                                                heading=oldPose[2],
                                                angleFB=oldAngles[0],
                                                angleLR=oldAngles[1],
                                                debugImg=debugImg,
                                                color=(0, 255, 255))
                            cv2.imwrite(debugFilename, debugImg)
                else:
                    print rects

                desiredSpeed = MAX_ALLOWED_SPEED
                if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
                    desiredSpeed = 0.0

                if drone.battery < 10:
                    print "BATTERY LOW!", drone.battery

                # height debugging
                #print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )

            # error definition ... if you substract that you get desired position or angle
            # error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
            errY, errA = 0.0, 0.0
            if refLine:
                errY = refLine.signedDistance(drone.coord)
                errA = normalizeAnglePIPI(drone.heading - refLine.angle)

            # get the height first
            if drone.coord[
                    2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
                sx = 0.0
            # error correction
            # the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
            sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0

            # there is no drone.va (i.e. derivative of heading) available at the moment ...
            sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
                desiredSpeed / 0.4)  # originally set for 0.4=OK

            #      print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
            prevTime = drone.time
            drone.moveXYZA(sx, sy, sz, sa)
            maxControlGap = max(drone.time - prevTime, maxControlGap)
            poseHistory.append(
                (drone.time, (drone.coord[0], drone.coord[1], drone.heading),
                 (drone.angleFB, drone.angleLR), altitude))
        print "NAVI-OFF", drone.time - startTime
        drone.hover(0.5)
        drone.land()
        drone.setVideoChannel(front=True)
    except ManualControlException, e:
        print "ManualControlException"
        if drone.ctrlState == 3:  # CTRL_FLYING=3 ... i.e. stop the current motion
            drone.hover(0.1)
        drone.land()
Example #7
0
def followRow( drone, desiredHeight = 1.5, timeout = 10.0 ): 
  maxControlGap = 0.0
  maxVideoDelay = 0.0
  desiredSpeed = MAX_ALLOWED_SPEED
  startTime = drone.time
  sx,sy,sz,sa = 0,0,0,0
  lastUpdate = None
  refLine = None
  while drone.time < startTime + timeout:
    altitude = desiredHeight
    if drone.altitudeData != None:
      altVision = drone.altitudeData[0]/1000.0
      altSonar = drone.altitudeData[3]/1000.0
      altitude = (altSonar+altVision)/2.0
      # TODO selection based on history? panic when min/max too far??
      if abs(altSonar-altVision) > 0.5:
#        print altSonar, altVision
        altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)
    sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
    if altitude > 2.5:
      # wind and "out of control"
      sz = max( -0.5, min( 0.5, desiredHeight - altitude ))

    if drone.lastImageResult:
      lastUpdate = drone.time
      assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
      (frameNumber, timestamp), rowTopBottom = drone.lastImageResult
      viewlog.dumpVideoFrame( frameNumber, timestamp )
      # keep history small
      videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
      videoDelay = drone.time - videoTime
      if videoDelay > 1.0:
        print "!DANGER! - video delay", videoDelay
      maxVideoDelay = max( videoDelay, maxVideoDelay )
      toDel = 0
      for oldTime, oldPose, oldAngles in drone.poseHistory:
        toDel += 1
        if oldTime >= videoTime:
          break
      drone.poseHistory = drone.poseHistory[:toDel]
      tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?      
      validRow = evalRowData( rowTopBottom )
      print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])), validRow
      if validRow:
        sp = groundPose( *rowTopBottom, scale=ROW_WIDTH/((validRow[0]+validRow[1])/2.0))
        sPose = Pose( *oldPose ).add(tiltCompensation).add( sp )
        refLine = Line( (sPose.x,sPose.y), (sPose.x + math.cos(sPose.heading), sPose.y + math.sin(sPose.heading)) )

    errY, errA = 0.0, 0.0
    if refLine:
      errY = refLine.signedDistance( drone.coord )
      errA = normalizeAnglePIPI( drone.heading - refLine.angle )


    sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
    sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
    sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4)
    prevTime = drone.time
    drone.moveXYZA( sx, sy, sz, sa )
    drone.poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
    maxControlGap = max( drone.time - prevTime, maxControlGap )
  return maxControlGap
Example #8
0
def competeAirRace(drone, desiredHeight=1.7):
    loops = []
    drone.speed = 0.1
    maxVideoDelay = 0.0
    maxControlGap = 0.0
    loc = StripsLocalisation()

    remainingFastStrips = 0
    desiredSpeed = TRANSITION_SPEED
    updateFailedCount = 0

    try:
        drone.wait(1.0)
        drone.setVideoChannel(front=False)
        drone.takeoff()
        poseHistory = []
        startTime = drone.time
        while drone.time < startTime + 1.0:
            drone.update("AT*PCMD=%i,0,0,0,0,0\r")  # drone.hover(1.0)
            poseHistory.append(
                (drone.time, (drone.coord[0], drone.coord[1], drone.heading),
                 (drone.angleFB, drone.angleLR)))
        magnetoOnStart = drone.magneto[:3]
        print "NAVI-ON"
        pathType = PATH_TURN_LEFT
        virtualRefCircle = None
        startTime = drone.time
        sx, sy, sz, sa = 0, 0, 0, 0
        lastUpdate = None
        while drone.time < startTime + 600.0:
            sz = max(-0.2, min(0.2, desiredHeight - drone.coord[2]))
            sx = max(0, min(drone.speed, desiredSpeed - drone.vx))

            if drone.lastImageResult:
                lastUpdate = drone.time
                assert len(drone.lastImageResult) == 2 and len(
                    drone.lastImageResult[0]) == 2, drone.lastImageResult
                (frameNumber, timestamp), rects = drone.lastImageResult
                viewlog.dumpCamera("tmp_%04d.jpg" % (frameNumber / 15, ), 0)

                # keep history small
                videoTime = correctTimePeriod(timestamp / 1000.,
                                              ref=drone.time)
                videoDelay = drone.time - videoTime
                if videoDelay > 1.0:
                    print "!DANGER! - video delay", videoDelay
                maxVideoDelay = max(videoDelay, maxVideoDelay)
                toDel = 0
                for oldTime, oldPose, oldAngles in poseHistory:
                    toDel += 1
                    if oldTime >= videoTime:
                        break
                poseHistory = poseHistory[:toDel]

                tiltCompensation = Pose(desiredHeight * oldAngles[0],
                                        desiredHeight * oldAngles[1],
                                        0)  # TODO real height?
                print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
                    oldAngles[0]), math.degrees(oldAngles[1])),
                loc.updateFrame(
                    Pose(*oldPose).add(tiltCompensation),
                    allStripPoses(rects,
                                  highResolution=drone.videoHighResolution))
                if loc.pathType != pathType:
                    print "TRANS", pathType, "->", loc.pathType
                    if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
                        if len(loops) > 0:
                            print "Loop %d, time %d" % (len(loops),
                                                        drone.time - loops[-1])
                        print "-----------------------------------------------"
                        loops.append(drone.time)
                    if drone.magneto[:3] == magnetoOnStart:
                        print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
                    pathType = loc.pathType
                    print "----"
                    remainingFastStrips = NUM_FAST_STRIPS

                if loc.crossing:
                    print "X", True, remainingFastStrips
                else:
                    print pathType, loc.pathUpdated, remainingFastStrips
                if not loc.pathUpdated:
                    updateFailedCount += 1
                    if updateFailedCount > 1:
                        print "UPDATE FAILED", updateFailedCount
                else:
                    updateFailedCount = 0

                if remainingFastStrips > 0:
                    remainingFastStrips -= 1
                    desiredSpeed = MAX_ALLOWED_SPEED
                    if not loc.pathUpdated and not loc.crossing:
                        desiredSpeed = STRIPS_FAILURE_SPEED
                else:
                    desiredSpeed = TRANSITION_SPEED
                if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
                    desiredSpeed = 0.0

                if drone.battery < 10:
                    print "BATTERY LOW!", drone.battery

                # height debugging
                #print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )

                for sp in allStripPoses(
                        rects, highResolution=drone.videoHighResolution):
                    sPose = Pose(*oldPose).add(tiltCompensation).add(sp)
                    viewlog.dumpBeacon(sPose.coord(), index=3)
                    viewlog.dumpObstacles([[
                        (sPose.x - 0.15 * math.cos(sPose.heading),
                         sPose.y - 0.15 * math.sin(sPose.heading)),
                        (sPose.x + 0.15 * math.cos(sPose.heading),
                         sPose.y + 0.15 * math.sin(sPose.heading))
                    ]])

            refCircle, refLine = loc.getRefCircleLine(
                Pose(drone.coord[0], drone.coord[1], drone.heading))
            if refCircle == None and refLine == None and virtualRefCircle != None:
                refCircle = virtualRefCircle
            # error definition ... if you substract that you get desired position or angle
            # error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
            errY, errA = 0.0, 0.0
            assert refCircle == None or refLine == None  # they cannot be both active at the same time
            if refCircle:
                if pathType == PATH_TURN_LEFT:
                    errY = refCircle[1] - math.hypot(
                        drone.coord[0] - refCircle[0][0],
                        drone.coord[1] - refCircle[0][1])
                    errA = normalizeAnglePIPI(
                        -math.atan2(refCircle[0][1] - drone.coord[1],
                                    refCircle[0][0] - drone.coord[0]) -
                        math.radians(-90) + drone.heading)
                if pathType == PATH_TURN_RIGHT:
                    errY = math.hypot(
                        drone.coord[0] - refCircle[0][0],
                        drone.coord[1] - refCircle[0][1]) - refCircle[1]
                    errA = normalizeAnglePIPI(
                        -math.atan2(refCircle[0][1] - drone.coord[1],
                                    refCircle[0][0] - drone.coord[0]) -
                        math.radians(90) + drone.heading)
            if refLine:
                errY = refLine.signedDistance(drone.coord)
                errA = normalizeAnglePIPI(drone.heading - refLine.angle)

            # get the height first
            if drone.coord[
                    2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
                sx = 0.0
            if refCircle == None and refLine == None and virtualRefCircle == None:
                sx = 0.0  # wait for Z-up
                if drone.coord[2] > desiredHeight - 0.1:
                    print "USING VIRTUAL LEFT TURN CIRCLE!"
                    circCenter = Pose(drone.coord[0], drone.coord[1],
                                      drone.heading).add(
                                          Pose(0.0, REF_CIRCLE_RADIUS,
                                               0)).coord()
                    viewlog.dumpBeacon(circCenter, index=0)
                    virtualRefCircle = circCenter, REF_CIRCLE_RADIUS

            # error correction
            # the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
            sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0

            # there is no drone.va (i.e. derivative of heading) available at the moment ...
            sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
                desiredSpeed / 0.4)  # originally set for 0.4=OK

            #      print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
            prevTime = drone.time
            drone.moveXYZA(sx, sy, sz, sa)
            maxControlGap = max(drone.time - prevTime, maxControlGap)
            poseHistory.append(
                (drone.time, (drone.coord[0], drone.coord[1], drone.heading),
                 (drone.angleFB, drone.angleLR)))
        print "NAVI-OFF", drone.time - startTime
        drone.hover(0.5)
        drone.land()
        drone.setVideoChannel(front=True)
    except ManualControlException, e:
        print "ManualControlException"
        if drone.ctrlState == 3:  # CTRL_FLYING=3 ... i.e. stop the current motion
            drone.hover(0.1)
        drone.land()
Example #9
0
def competeAirRace( drone, desiredHeight = 1.7 ):
  loops = []
  drone.speed = 0.1
  maxVideoDelay = 0.0
  maxControlGap = 0.0
  loc = StripsLocalisation()
  
  remainingFastStrips = 0
  desiredSpeed = TRANSITION_SPEED
  updateFailedCount = 0

  try:
    drone.wait(1.0)
    drone.setVideoChannel( front=False )    
    drone.takeoff()
    poseHistory = []
    startTime = drone.time
    while drone.time < startTime + 1.0:
      drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
      poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
    magnetoOnStart = drone.magneto[:3]
    print "NAVI-ON"
    pathType = PATH_TURN_LEFT
    virtualRefCircle = None
    startTime = drone.time
    sx,sy,sz,sa = 0,0,0,0
    lastUpdate = None
    while drone.time < startTime + 600.0:
      sz = max( -0.2, min( 0.2, desiredHeight - drone.coord[2] ))
      sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))

      if drone.lastImageResult:
        lastUpdate = drone.time
        assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
        (frameNumber, timestamp), rects = drone.lastImageResult
        viewlog.dumpCamera( "tmp_%04d.jpg" % (frameNumber/15,), 0 )

        # keep history small
        videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
        videoDelay = drone.time - videoTime
        if videoDelay > 1.0:
          print "!DANGER! - video delay", videoDelay
        maxVideoDelay = max( videoDelay, maxVideoDelay )
        toDel = 0
        for oldTime, oldPose, oldAngles in poseHistory:
          toDel += 1
          if oldTime >= videoTime:
            break
        poseHistory = poseHistory[:toDel]

        tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
        print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
        loc.updateFrame( Pose( *oldPose ).add(tiltCompensation), allStripPoses( rects, highResolution=drone.videoHighResolution ) )
        if loc.pathType != pathType:
          print "TRANS", pathType, "->", loc.pathType
          if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
            if len(loops) > 0:
              print "Loop %d, time %d" % (len(loops), drone.time-loops[-1])
            print "-----------------------------------------------"
            loops.append( drone.time )
          if drone.magneto[:3] == magnetoOnStart:
            print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
          pathType = loc.pathType
          print "----"
          remainingFastStrips = NUM_FAST_STRIPS

        if loc.crossing:
          print "X", True, remainingFastStrips
        else:
          print pathType, loc.pathUpdated, remainingFastStrips
        if not loc.pathUpdated:
          updateFailedCount += 1
          if updateFailedCount > 1:
            print "UPDATE FAILED", updateFailedCount
        else:
          updateFailedCount = 0

        if remainingFastStrips > 0:
          remainingFastStrips -= 1
          desiredSpeed = MAX_ALLOWED_SPEED
          if not loc.pathUpdated and not loc.crossing:
            desiredSpeed = STRIPS_FAILURE_SPEED
        else:
          desiredSpeed = TRANSITION_SPEED
        if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
          desiredSpeed = 0.0

        if drone.battery < 10:
          print "BATTERY LOW!", drone.battery

        # height debugging
        #print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )

        for sp in allStripPoses( rects, highResolution=drone.videoHighResolution ):
          sPose = Pose( *oldPose ).add(tiltCompensation).add( sp )
          viewlog.dumpBeacon( sPose.coord(), index=3 )
          viewlog.dumpObstacles( [[(sPose.x-0.15*math.cos(sPose.heading), sPose.y-0.15*math.sin(sPose.heading)), 
                                       (sPose.x+0.15*math.cos(sPose.heading), sPose.y+0.15*math.sin(sPose.heading))]] )

      refCircle,refLine = loc.getRefCircleLine( Pose(drone.coord[0], drone.coord[1], drone.heading) )
      if refCircle == None and refLine == None and virtualRefCircle != None:
        refCircle = virtualRefCircle
      # error definition ... if you substract that you get desired position or angle
      # error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
      errY, errA = 0.0, 0.0
      assert refCircle == None or refLine == None # they cannot be both active at the same time
      if refCircle:
        if pathType == PATH_TURN_LEFT:
          errY = refCircle[1] - math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] )
          errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] ) 
                                      - math.radians(-90) + drone.heading )
        if pathType == PATH_TURN_RIGHT:
          errY = math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] ) - refCircle[1]
          errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] ) 
                                      - math.radians(90) + drone.heading )
      if refLine:
        errY = refLine.signedDistance( drone.coord )
        errA = normalizeAnglePIPI( drone.heading - refLine.angle )

      # get the height first
      if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
        sx = 0.0
      if refCircle == None and refLine == None and virtualRefCircle == None:
        sx = 0.0 # wait for Z-up
        if drone.coord[2] > desiredHeight - 0.1:
          print "USING VIRTUAL LEFT TURN CIRCLE!"
          circCenter = Pose( drone.coord[0], drone.coord[1], drone.heading ).add( Pose(0.0, REF_CIRCLE_RADIUS, 0 )).coord()
          viewlog.dumpBeacon( circCenter, index=0 )
          virtualRefCircle = circCenter, REF_CIRCLE_RADIUS

      # error correction
      # the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
      sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
      
      # there is no drone.va (i.e. derivative of heading) available at the moment ... 
      sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK

#      print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
      prevTime = drone.time
      drone.moveXYZA( sx, sy, sz, sa )
      maxControlGap = max( drone.time - prevTime, maxControlGap )
      poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
    print "NAVI-OFF", drone.time - startTime
    drone.hover(0.5)
    drone.land()
    drone.setVideoChannel( front=True )    
  except ManualControlException, e:
    print "ManualControlException"
    if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
      drone.hover(0.1)
    drone.land()