def testCapture():
vc = VideoCapture(outfile="sample_video/other_side_test3.avi")
while vc.captureFrame():
vc.writeFrame()
vc.saveFrameToBuf()
if vc.frameCount > 50:
break
print "sending start"
cl.flush()
cl.start(13)
test = cl.readAndCheck()
if not test:
print "start not received D="
else:
print "start received!"
while vc.captureFrame():
vc.writeFrame()
vc.saveFrameToBuf()
if vc.frameCount > 250:
break
delay(3000)
print "sending stop"
cl.flush()
cl.stop()
if not cl.readAndCheck():
print "stop not received D="
else:
print "stop received!"
def testCapture(): vc = VideoCapture( outfile="sample_video/other_side_test3.avi" ) while vc.captureFrame(): vc.writeFrame() vc.saveFrameToBuf() if vc.frameCount > 50: break print "sending start" cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" while vc.captureFrame(): vc.writeFrame() vc.saveFrameToBuf() if vc.frameCount > 250: break delay( 3000 ) print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!"
def simpleNudgeTest():
print "sending start"
cl.flush()
cl.start(13)
test = cl.readAndCheck()
if not test:
print "start not received D="
else:
print "start received!"
delay(9000)
print "nudging"
cl.flush()
cl.nudge(cl.M1)
test = cl.readAndCheck()
if not test:
print "nudge not received D="
else:
print "nudge received!"
delay(3000)
print "sending stop"
cl.flush()
cl.stop()
if not cl.readAndCheck():
print "stop not received D="
else:
print "stop received!"
def simpleNudgeTest(): print "sending start" cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" delay( 9000 ) print "nudging" cl.flush() cl.nudge( cl.M1 ) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" delay( 3000 ) print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!"
def mainLoop():
vc = VideoCapture(outfile="sample_video/nudge_movement_raw.avi")
lastNudge = 0
intersect = None
print "startup loop"
while vc.frameBuf.size() < 5 and vc.captureFrame():
frame, intersect, horz = vc.findLines()
vc.drawGrid(frame)
vc.writeFrame()
if intersect:
vc.saveFrameToBuf()
print "sending start"
cl.flush()
cl.start(13)
test = cl.readAndCheck()
if not test:
print "start not received D="
else:
print "start received!"
# t0 = time.time()
while vc.captureFrame() and vc.frameCount < 150:
frame, intersect, horz = vc.findLines()
nudgeMotor = None
nudgeTime = None
mustStop = False
for line in horz:
# print "horz:", line
if line[0] > vc.height - 50 and line[1] == 255:
print "can't continue, gotta stop"
mustStop = True
break
if mustStop:
vc.drawGrid(frame)
vc.writeFrame()
vc.saveFrameToBuf()
break
if intersect and vc.frameCount >= (lastNudge + 15):
if intersect > (vc.width / 2 + 60):
print "BIGGER NUDGE M2!!"
nudgeMotor = cl.M2
nudgeTime = 7
circle(frame, (20, vc.height - 20), 6, (0, 0, 255), 2)
elif intersect > (vc.width / 2 + 30):
print "BIG NUDGE M2!!"
nudgeMotor = cl.M2
nudgeTime = 4
circle(frame, (20, vc.height - 20), 4, (0, 0, 255), 2)
elif intersect > (vc.width / 2 + 15):
print "NUDGE M2!!"
nudgeMotor = cl.M2
nudgeTime = 2
circle(frame, (20, vc.height - 20), 2, (0, 0, 255), 2)
elif intersect < (vc.width / 2 - 60):
print "BIGGER NUDGE M1!!"
nudgeMotor = cl.M1
nudgeTime = 7
circle(frame, (20, vc.height - 20), 6, (0, 255, 0), 2)
elif intersect < (vc.width / 2 - 30):
print "BIG NUDGE M1!!"
nudgeMotor = cl.M1
nudgeTime = 4
circle(frame, (20, vc.height - 20), 4, (0, 255, 0), 2)
elif intersect < (vc.width / 2 - 15):
print "NUDGE M1!!"
nudgeMotor = cl.M1
nudgeTime = 2
circle(frame, (20, vc.height - 20), 2, (0, 255, 0), 2)
if nudgeMotor and nudgeTime:
cl.flush()
cl.nudge(nudgeMotor, nudgeTime)
test = cl.readAndCheck()
if not test:
print "nudge not received D="
else:
print "nudge received!"
lastNudge = vc.frameCount
vc.drawGrid(frame)
vc.writeFrame()
vc.saveFrameToBuf()
print "sending stop"
cl.flush()
cl.stop()
if not cl.readAndCheck():
print "stop not received D="
else:
print "stop received!"
# get a few final frames
print "get a few more frames"
marker = vc.frameCount + 10
while vc.captureFrame() and vc.frameCount < marker:
frame, intersect, horz = vc.findLines()
vc.drawGrid(frame)
vc.writeFrame(frame)
vc.saveFrameToBuf()
def mainLoop(): vc = VideoCapture( outfile="sample_video/nudge_movement_raw.avi" ) lastNudge = 0 intersect = None print "startup loop" while vc.frameBuf.size() < 5 and vc.captureFrame(): frame, intersect, horz = vc.findLines() vc.drawGrid( frame ) vc.writeFrame( ) if intersect: vc.saveFrameToBuf() print "sending start" cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" # t0 = time.time() while vc.captureFrame() and vc.frameCount < 150: frame, intersect, horz = vc.findLines() nudgeMotor = None nudgeTime = None mustStop = False for line in horz: # print "horz:", line if line[0] > vc.height-50 and line[1] == 255: print "can't continue, gotta stop" mustStop = True break if mustStop: vc.drawGrid( frame ) vc.writeFrame( ) vc.saveFrameToBuf() break if intersect and vc.frameCount >= (lastNudge+15): if intersect > (vc.width/2 + 60): print "BIGGER NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 7 circle( frame, (20, vc.height-20), 6, (0,0,255), 2 ) elif intersect > (vc.width/2 + 30): print "BIG NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,0,255), 2 ) elif intersect > (vc.width/2 + 15): print "NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,0,255), 2 ) elif intersect < (vc.width/2 - 60): print "BIGGER NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 7 circle( frame, (20, vc.height-20), 6, (0,255,0), 2 ) elif intersect < (vc.width/2 - 30): print "BIG NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,255,0), 2 ) elif intersect < (vc.width/2 - 15): print "NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,255,0), 2 ) if nudgeMotor and nudgeTime: cl.flush() cl.nudge( nudgeMotor, nudgeTime ) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" lastNudge = vc.frameCount vc.drawGrid( frame ) vc.writeFrame( ) vc.saveFrameToBuf() print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!" # get a few final frames print "get a few more frames" marker = vc.frameCount + 10 while vc.captureFrame() and vc.frameCount < marker: frame, intersect, horz = vc.findLines() vc.drawGrid( frame ) vc.writeFrame( frame ) vc.saveFrameToBuf()
def mainLoop():
global nextTurn
while True:
Log.debug("buffer loop")
while vc.frameBuf.size(
) < 5 and vc.lastFlowPnts is None and vc.captureFrame():
lineFrame, intersect, horz = vc.findLines()
cornerFrame = vc.trackCorners()
vc.drawGrid(lineFrame)
vc.writeFrame(lineFrame)
if intersect:
vc.saveFrameToBuf()
# check allowed directions
directions = vc.findTurns()
Log.debug(directions)
if len(directions) == 0:
Log.warning("could not find directions, trying again..")
vc.reset()
continue
directions.append('Down')
nextTurn = random.choice(directions)
# next = vc.findShapes()
# if next == 'Destination':
# nextTurn = next
# elif next in directions:
# nextTurn = next
# else:
# nextTurn = random.choice( directions )
Log.info("Nest turn is: " + nextTurn)
Log.debug("sending start")
cl.flush()
cl.start(13)
test = cl.readAndCheck()
if not test:
Log.warning("start not received D=")
else:
Log.info("start received!")
# go forward til we have to stop
if not forwardMovement():
# stop and reset
Log.debug("sending stop")
cl.flush()
cl.stop()
if not cl.readAndCheck():
Log.warning("stop not received D=")
else:
Log.info("stop received!")
delay(100)
vc.reset()
continue
Log.debug("sending stop")
cl.flush()
cl.stop()
if not cl.readAndCheck():
Log.warning("stop not received D=")
else:
Log.info("stop received!")
delay(100)
# now we gotta turn
if nextTurn == 'Left':
Log.debug("sending Left")
cl.flush()
cl.turnLeft()
delay(4000)
if not cl.readAndCheck():
Log.warning("Left not received D=")
else:
Log.info("Left received!")
elif nextTurn == 'Right':
Log.debug("sending Right")
cl.flush()
cl.turnRight()
delay(4000)
if not cl.readAndCheck():
Log.warning("Right not received D=")
else:
Log.info("Right received!")
elif nextTurn == 'Up':
Log.debug("sending Up")
elif nextTurn == 'Down':
Log.debug("sending Down")
cl.flush()
cl.turnAround()
delay(4000)
if not cl.readAndCheck():
Log.warning("Turn around not received D=")
else:
Log.info("Turn around received!")
elif nextTurn == 'StopSign':
Log.debug("sending StopSign")
cl.flush()
#cl.stop()
if not cl.readAndCheck():
Log.warning("Stop sign not received D=")
else:
Log.info("Stop sign received!")
elif nextTurn == 'Destination':
Log.debug("sending Destination")
cl.flush()
cl.stop()
if not cl.readAndCheck():
Log.warning("Destination not received D=")
else:
Log.info("Destination received!")
# burn some frames
Log.info("burning frames..")
for i in range(10):
vc.captureFrame()
Log.info("resetting video capture")
vc.reset()
def forwardMovement():
global nextTurn
lastNudge = 0
intersect = None
while vc.captureFrame():
lineFrame, intersect, horz = vc.findLines()
if vc.checkReset():
Log.info("resetting....")
return False
# vc.frameBuf.printHorzDiff()
# if nextTurn == None:
# next = vc.findShapes()
cornerFrame = vc.trackCorners()
vc.drawGrid(lineFrame)
vc.writeFrame(lineFrame)
vc.saveFrameToBuf()
frame = lineFrame
nudgeMotor = None
nudgeTime = None
if vc.lastFlowPnts is not None:
pnts = vc.lastFlowPnts
# Log.debug( 3*vc.height/4 )
# Log.debug( pnt )
if len(pnts) == 2 and abs(pnts[0][0][1] - pnts[1][0][1]) < 10:
# condition where there are only 2 dots on the same horz
# we don't wanna stop on the farthest tracked point
pass
elif pnts[0][0][1] > (3 * vc.height / 4) - 25:
Log.debug("can't continue, gotta stop - corner")
return True
for line in horz:
if line[0] > vc.height - 80 and line[1] == 255:
Log.debug("can't continue, gotta stop - line")
return True
if intersect and vc.frameCount >= (lastNudge + 10):
if intersect > (vc.width / 2 + 60):
nudgeMotor = cl.M2
nudgeTime = 6
circle(frame, (20, vc.height - 20), 6, (0, 0, 255), 2)
elif intersect > (vc.width / 2 + 25):
nudgeMotor = cl.M2
nudgeTime = 4
circle(frame, (20, vc.height - 20), 4, (0, 0, 255), 2)
elif intersect > (vc.width / 2 + 15):
nudgeMotor = cl.M2
nudgeTime = 2
circle(frame, (20, vc.height - 20), 2, (0, 0, 255), 2)
elif intersect < (vc.width / 2 - 60):
nudgeMotor = cl.M1
nudgeTime = 6
circle(frame, (20, vc.height - 20), 6, (0, 255, 0), 2)
elif intersect < (vc.width / 2 - 25):
nudgeMotor = cl.M1
nudgeTime = 4
circle(frame, (20, vc.height - 20), 4, (0, 255, 0), 2)
elif intersect < (vc.width / 2 - 15):
nudgeMotor = cl.M1
nudgeTime = 2
circle(frame, (20, vc.height - 20), 2, (0, 255, 0), 2)
if nudgeMotor and nudgeTime:
cl.flush()
cl.nudge(nudgeMotor, nudgeTime)
test = cl.readAndCheck()
if not test:
Log.warning("nudge %s, %d not received D=" %
(nudgeMotor, nudgeTime))
else:
Log.info("nudge %s, %d received!" % (nudgeMotor, nudgeTime))
lastNudge = vc.frameCount
def mainLoop(): global nextTurn while True: Log.debug( "buffer loop" ) while vc.frameBuf.size() < 5 and vc.lastFlowPnts is None and vc.captureFrame(): lineFrame, intersect, horz = vc.findLines() cornerFrame = vc.trackCorners() vc.drawGrid( lineFrame ) vc.writeFrame( lineFrame ) if intersect: vc.saveFrameToBuf() # check allowed directions directions = vc.findTurns() Log.debug( directions ) if len( directions ) == 0: Log.warning( "could not find directions, trying again.." ) vc.reset() continue directions.append( 'Down' ) nextTurn = random.choice( directions ) # next = vc.findShapes() # if next == 'Destination': # nextTurn = next # elif next in directions: # nextTurn = next # else: # nextTurn = random.choice( directions ) Log.info( "Nest turn is: " + nextTurn ) Log.debug( "sending start" ) cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: Log.warning( "start not received D=" ) else: Log.info( "start received!" ) # go forward til we have to stop if not forwardMovement(): # stop and reset Log.debug( "sending stop" ) cl.flush() cl.stop() if not cl.readAndCheck(): Log.warning( "stop not received D=" ) else: Log.info( "stop received!" ) delay( 100 ) vc.reset() continue Log.debug( "sending stop" ) cl.flush() cl.stop() if not cl.readAndCheck(): Log.warning( "stop not received D=" ) else: Log.info( "stop received!" ) delay( 100 ) # now we gotta turn if nextTurn == 'Left': Log.debug( "sending Left" ) cl.flush() cl.turnLeft() delay( 4000 ) if not cl.readAndCheck(): Log.warning( "Left not received D=" ) else: Log.info( "Left received!" ) elif nextTurn == 'Right': Log.debug( "sending Right" ) cl.flush() cl.turnRight() delay( 4000 ) if not cl.readAndCheck(): Log.warning( "Right not received D=" ) else: Log.info( "Right received!" ) elif nextTurn == 'Up': Log.debug( "sending Up" ) elif nextTurn == 'Down': Log.debug( "sending Down" ) cl.flush() cl.turnAround() delay( 4000 ) if not cl.readAndCheck(): Log.warning( "Turn around not received D=" ) else: Log.info( "Turn around received!" ) elif nextTurn == 'StopSign': Log.debug( "sending StopSign" ) cl.flush() #cl.stop() if not cl.readAndCheck(): Log.warning( "Stop sign not received D=" ) else: Log.info( "Stop sign received!" ) elif nextTurn == 'Destination': Log.debug( "sending Destination" ) cl.flush() cl.stop() if not cl.readAndCheck(): Log.warning( "Destination not received D=" ) else: Log.info( "Destination received!" ) # burn some frames Log.info( "burning frames.." ) for i in range(10): vc.captureFrame() Log.info( "resetting video capture" ) vc.reset()
def forwardMovement(): global nextTurn lastNudge = 0 intersect = None while vc.captureFrame(): lineFrame, intersect, horz = vc.findLines() if vc.checkReset(): Log.info( "resetting...." ) return False # vc.frameBuf.printHorzDiff() # if nextTurn == None: # next = vc.findShapes() cornerFrame = vc.trackCorners() vc.drawGrid( lineFrame ) vc.writeFrame( lineFrame ) vc.saveFrameToBuf() frame = lineFrame nudgeMotor = None nudgeTime = None if vc.lastFlowPnts is not None: pnts = vc.lastFlowPnts # Log.debug( 3*vc.height/4 ) # Log.debug( pnt ) if len(pnts) == 2 and abs(pnts[0][0][1] - pnts[1][0][1]) < 10: # condition where there are only 2 dots on the same horz # we don't wanna stop on the farthest tracked point pass elif pnts[0][0][1] > (3*vc.height/4)-25: Log.debug( "can't continue, gotta stop - corner" ) return True for line in horz: if line[0] > vc.height-80 and line[1] == 255: Log.debug( "can't continue, gotta stop - line" ) return True if intersect and vc.frameCount >= (lastNudge+10): if intersect > (vc.width/2 + 60): nudgeMotor = cl.M2 nudgeTime = 6 circle( frame, (20, vc.height-20), 6, (0,0,255), 2 ) elif intersect > (vc.width/2 + 25): nudgeMotor = cl.M2 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,0,255), 2 ) elif intersect > (vc.width/2 + 15): nudgeMotor = cl.M2 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,0,255), 2 ) elif intersect < (vc.width/2 - 60): nudgeMotor = cl.M1 nudgeTime = 6 circle( frame, (20, vc.height-20), 6, (0,255,0), 2 ) elif intersect < (vc.width/2 - 25): nudgeMotor = cl.M1 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,255,0), 2 ) elif intersect < (vc.width/2 - 15): nudgeMotor = cl.M1 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,255,0), 2 ) if nudgeMotor and nudgeTime: cl.flush() cl.nudge( nudgeMotor, nudgeTime ) test = cl.readAndCheck() if not test: Log.warning( "nudge %s, %d not received D=" % (nudgeMotor, nudgeTime) ) else: Log.info( "nudge %s, %d received!" % (nudgeMotor, nudgeTime) ) lastNudge = vc.frameCount