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