Ejemplo n.º 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
Ejemplo n.º 2
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()
Ejemplo n.º 3
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()
Ejemplo n.º 4
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
Ejemplo n.º 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