def __init__( self, config, logger, playerPos3d,
playerDepthSensor, playerSonar, playerFrontCamera ):
ControlScript.__init__( self, config, logger,
playerPos3d, playerCompass = None,
playerDepthSensor = playerDepthSensor, playerSonar = playerSonar,
playerFrontCamera = playerFrontCamera )
self.sonarScanner = SonarScanner(
logger, playerSonar,
playerDepthSensor, config )
self.setState( self.STATE_PERFORMING_SCANS )
self.lastCameraFrameTime = 0.0
self.lastSonarFrameTime = 0.0
self.lastCameraImageSaveTime = 0.0
self.sonarImagesDir = string.Template( config.sonarImagesDir ).safe_substitute( os.environ )
self.cameraImagesDir = string.Template( config.cameraImagesDir ).safe_substitute( os.environ )
self.sonarScanner.setSonarConfig(
self.config.IC_Script_sonarRange,
self.config.IC_Script_sonarNumBins,
self.config.IC_Script_sonarGain )
self.sonarScanner.setScanAngleRange(
Maths.degToRad( self.config.IC_Script_sonarScanStartAngleDegrees ),
Maths.degToRad( self.config.IC_Script_sonarScanEndAngleDegrees ) )
self.bgrImage = None
def __init__( self, logger, playerCompass, playerSonar,
playerDepthSensor = None, config = SubControllerConfig() ):
self.logger = logger
self.config = config
self.playerCompass = playerCompass
self.playerSonar = playerSonar
self.lastSonarFrameTime = 0
self.active = True
self.sonarScanner = SonarScanner( logger, playerSonar,
playerDepthSensor, config )
self.sonarScanner.setActive( False )
self.subPos = None
self.detectedLines = None
self.thresholdedSonarImage = None
self.cornerPos = None
self.sonarImage = None
self.sonarImagesDir = string.Template( config.sonarImagesDir ).safe_substitute( os.environ )
class ImageCaptureScript( ControlScript ):
STATE_PERFORMING_SCANS = "Performing Scans"
#---------------------------------------------------------------------------
def __init__( self, config, logger, playerPos3d,
playerDepthSensor, playerSonar, playerFrontCamera ):
ControlScript.__init__( self, config, logger,
playerPos3d, playerCompass = None,
playerDepthSensor = playerDepthSensor, playerSonar = playerSonar,
playerFrontCamera = playerFrontCamera )
self.sonarScanner = SonarScanner(
logger, playerSonar,
playerDepthSensor, config )
self.setState( self.STATE_PERFORMING_SCANS )
self.lastCameraFrameTime = 0.0
self.lastSonarFrameTime = 0.0
self.lastCameraImageSaveTime = 0.0
self.sonarImagesDir = string.Template( config.sonarImagesDir ).safe_substitute( os.environ )
self.cameraImagesDir = string.Template( config.cameraImagesDir ).safe_substitute( os.environ )
self.sonarScanner.setSonarConfig(
self.config.IC_Script_sonarRange,
self.config.IC_Script_sonarNumBins,
self.config.IC_Script_sonarGain )
self.sonarScanner.setScanAngleRange(
Maths.degToRad( self.config.IC_Script_sonarScanStartAngleDegrees ),
Maths.degToRad( self.config.IC_Script_sonarScanEndAngleDegrees ) )
self.bgrImage = None
#---------------------------------------------------------------------------
def isNewSonarFrameAvailable( self ):
if self.playerSonar == None:
return False
else:
return self.lastSonarFrameTime != self.sonarScanner.lastSonarFrameTime
#---------------------------------------------------------------------------
def isNewCameraFrameAvailable( self ):
if self.playerFrontCamera == None:
return False
else:
return self.lastCameraFrameTime != self.playerFrontCamera.info.datatime
#---------------------------------------------------------------------------
def update( self ):
curTime = time.time()
if self.state == self.STATE_PERFORMING_SCANS:
if self.config.IC_Script_enableSonar:
self.sonarScanner.update()
if self.isNewSonarFrameAvailable():
self.lastSonarFrameTime = self.sonarScanner.lastSonarFrameTime
# Save the sonar image
sonarImageFilename = "{0}/{1}.png".format(
self.sonarImagesDir, str( datetime.now() ) )
cv.SaveImage( sonarImageFilename, self.sonarScanner.sonarImage )
if self.config.IC_Script_enableCamera:
if curTime - self.lastCameraImageSaveTime > 1.0/self.config.IC_Script_numImagesSavedPerSecond \
and self.isNewCameraFrameAvailable():
self.lastCameraFrameTime = self.playerFrontCamera.info.datatime
self.lastCameraImageSaveTime = curTime
# Save the camera image
cameraImageFilename = "{0}/{1}.png".format(
self.cameraImagesDir, str( datetime.now() ) )
if self.playerFrontCamera.compression != playerc.PLAYER_CAMERA_COMPRESS_RAW:
self.playerFrontCamera.decompress()
if self.playerFrontCamera.compression != playerc.PLAYER_CAMERA_COMPRESS_RAW:
print "Error: Unable to decompress frame"
sys.exit( -1 )
# Give the image to OpenCV as a very inefficient way to
# save it as a jpeg
rgbImage = cv.CreateImageHeader( ( self.playerFrontCamera.width, self.playerFrontCamera.height ), cv.IPL_DEPTH_8U, 3 )
cv.SetData( rgbImage, self.playerFrontCamera.image[:self.playerFrontCamera.image_count], self.playerFrontCamera.width*3 )
# Resize the image to 160x120
zoom = 0.5
if zoom != 1.0:
scaledImage = cv.CreateImage(
( int( rgbImage.width*zoom ),
int ( rgbImage.height*zoom ) ),
rgbImage.depth, rgbImage.nChannels )
cv.Resize( rgbImage, scaledImage )
rgbImage = scaledImage
# Image must be converted to bgr for saving with OpenCV
self.bgrImage = cv.CreateImage( ( rgbImage.width, rgbImage.height ), cv.IPL_DEPTH_8U, 3 )
cv.CvtColor( rgbImage, self.bgrImage, cv.CV_RGB2BGR )
cv.SaveImage( cameraImageFilename, self.bgrImage )
class SonarLocator:
SCAN_RANGE = 70
NUM_BINS = 300
GAIN = 0.4
# All angles here are like compass bearings so 0 degrees is north
# and angles increase in a clock-wise direction
SAVE_IMAGES = True
SONAR_HEADING_OFFSET = Maths.degToRad( 180.0 ) # Offset from sub heading to sonar
POOL_HEADING = Maths.degToRad( 0.0 ) # Heading of the pool in degrees converted to radians
IDEAL_SCAN_START_ANGLE = Maths.degToRad( 170.0 )
IDEAL_SCAN_END_ANGLE = Maths.degToRad( 280.0 )
#---------------------------------------------------------------------------
def __init__( self, logger, playerCompass, playerSonar,
playerDepthSensor = None, config = SubControllerConfig() ):
self.logger = logger
self.config = config
self.playerCompass = playerCompass
self.playerSonar = playerSonar
self.lastSonarFrameTime = 0
self.active = True
self.sonarScanner = SonarScanner( logger, playerSonar,
playerDepthSensor, config )
self.sonarScanner.setActive( False )
self.subPos = None
self.detectedLines = None
self.thresholdedSonarImage = None
self.cornerPos = None
self.sonarImage = None
self.sonarImagesDir = string.Template( config.sonarImagesDir ).safe_substitute( os.environ )
#---------------------------------------------------------------------------
def setActive( self, active ):
self.active = active
if not self.active:
self.cornerPos = None
#---------------------------------------------------------------------------
def isNewSonarFrameAvailable( self ):
if self.playerSonar == None:
return False
else:
return self.lastSonarFrameTime != self.sonarScanner.lastSonarFrameTime
#---------------------------------------------------------------------------
def configureSonar( self ):
subHeading = self.playerCompass.pose.pyaw
angleOffset = subHeading - self.POOL_HEADING # Diff from pool to sub heading
# Scan when the sub is aligned with the pool to find the bottom right corner
scanStartAngle = self.SONAR_HEADING_OFFSET + self.IDEAL_SCAN_START_ANGLE - angleOffset
scanEndAngle = self.SONAR_HEADING_OFFSET + self.IDEAL_SCAN_END_ANGLE - angleOffset
scanStartAngle = Maths.normaliseAngle( scanStartAngle, 0.0 )
scanEndAngle = Maths.normaliseAngle( scanEndAngle, 0.0 )
#print "From", Maths.radToDeg( startScanAngle ), "To", Maths.radToDeg( endScanAngle )
# Configure the sonar scanner
self.sonarScanner.setSonarConfig( self.SCAN_RANGE, self.NUM_BINS, self.GAIN )
self.sonarScanner.setScanAngleRange( scanStartAngle, scanEndAngle )
#---------------------------------------------------------------------------
def update( self ):
if not self.active:
return
if self.playerCompass.info.datatime != 0:
self.compassStartTime = self.playerCompass.info.datatime
else:
# Don't start until we have compass data
return
self.sonarScanner.update()
if not self.sonarScanner.active:
self.configureSonar()
self.sonarScanner.setActive( True )
else:
if self.isNewSonarFrameAvailable():
self.configureSonar() # Prepare for next sonar scan
self.lastSonarFrameTime = self.sonarScanner.lastSonarFrameTime
self.sonarImage = self.sonarScanner.sonarImage
# Save the sonar image
if self.SAVE_IMAGES:
sonarImageFilename = "{0}/corner_{1}.png".format(
self.sonarImagesDir, str( datetime.now() ) )
cv.SaveImage( sonarImageFilename, self.sonarImage )
# Run the corner finder on the image
#self.detectedLines, self.thresholdedSonarImage, self.cornerPos = \
# CF_FindCorner( self.sonarImage )
if self.cornerPos != None:
#self.subPos
pass
