<s> import time <EOL> from director import segmentationroutines <EOL> from director import segmentation <EOL> from director . timercallback import TimerCallback <EOL> from director . visualization import * <EOL> class TrackDrillOnTable ( object ) : <EOL> def __init__ ( self ) : <EOL> self . tableCentroid = None <EOL> def updateFit ( self ) : <EOL> polyData = segmentation . getDisparityPointCloud ( ) <EOL> if ( polyData is None ) : <EOL> return <EOL> updatePolyData ( polyData , '<STR_LIT>' , colorByName = '<STR_LIT>' , visible = False ) <EOL> t0 = time . time ( ) <EOL> if ( self . tableCentroid is None ) : <EOL> print "<STR_LIT>" <EOL> self . tableCentroid = segmentation . findAndFitDrillBarrel ( polyData ) <EOL> else : <EOL> viewFrame = segmentationroutines . SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> forwardDirection = np . array ( [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewFrame . TransformVector ( forwardDirection , forwardDirection ) <EOL> robotOrigin = viewFrame . GetPosition ( ) <EOL> robotForward = forwardDirection <EOL> fitResults = [ ] <EOL> drillFrame = segmentation . segmentDrillBarrelFrame ( self . tableCentroid , polyData , robotForward ) <EOL> clusterObj = updatePolyData ( polyData , '<STR_LIT>' , color = [ <NUM_LIT:1> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = segmentation . getDebugFolder ( ) , visible = False ) <EOL> fitResults . append ( ( clusterObj , drillFrame ) ) <EOL> segmentation . sortFittedDrills ( fitResults , robotOrigin , robotForward ) <EOL> class PointerTracker ( object ) : <EOL> '''<STR_LIT>''' <EOL> def __init__ ( self , robotModel , stereoPointCloudItem ) : <EOL> self . robotModel = robotModel <EOL> self . stereoPointCloudItem = stereoPointCloudItem <EOL> self . timer = TimerCallback ( targetFps = <NUM_LIT:5> ) <EOL> self . timer . callback = self . updateFit <EOL> def start ( self ) : <EOL> self . timer . start ( ) <EOL> def stop ( self ) : <EOL> self . timer . stop ( ) <EOL> def cleanup ( self ) : <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> def updateFit ( self , polyData = None ) : <EOL> if not polyData : <EOL> self . stereoPointCloudItem . update ( ) <EOL> polyData = self . stereoPointCloudItem . polyData <EOL> if not polyData or not polyData . GetNumberOfPoints ( ) : <EOL> self . cleanup ( ) <EOL> return <EOL> self . tipPosition = segmentation . estimatePointerTip ( self . robotModel , polyData ) <EOL> if self . tipPosition is None : <EOL> self . cleanup ( ) <EOL> def getPointerTip ( self ) : <EOL> return self . tipPosition </s>
