def getVisibleMarkers(self): # !!! ,robot): robot = self.robots[0] foundMarkers = [] # print "robot", robot._gx, robot._gy, robot._ga for (markerX,markerY) in self.markerLoc.keys(): markerName = self.markerLoc[(markerX,markerY)] markerAngle = self.markerAngle[markerName] # print "marker", markerName, markerX, markerY, markerAngle distanceToMarker = calculateHypotenuse(robot._gx-markerX, robot._gy-markerY) # get the angle to the marker, and its relation to the # orientation of the robot angleToMarker = getAbsAngle(centerX=robot._gx, centerY=robot._gy, headingX=markerX, headingY=markerY) cameraAngleDiff = normalizeAngle2(angleToMarker-robot._ga+math.pi) # we do the same but this time from the perspective of the # marker angleFromMarker = getAbsAngle(centerX=markerX, centerY=markerY, headingX=robot._gx, headingY=robot._gy) markerViewDiff = normalizeAngle2(angleFromMarker-markerAngle+math.pi) # print angleToMarker,cameraAngleDiff,",",angleFromMarker,markerViewDiff # see if the marker is in the camera's field of view # CAMERA_FIELD_OF_VIEW/2 = 0.35 # and that the marker is at a viewable angle from the robot # MARKER_VIEWABLE_ANGLE/2 = 0.52 if (cameraAngleDiff < self.CAMERA_FIELD_OF_VIEW/2 and cameraAngleDiff > self.CAMERA_FIELD_OF_VIEW/2*-1 and markerViewDiff < self.MARKER_VIEWABLE_ANGLE/2 and markerViewDiff > self.MARKER_VIEWABLE_ANGLE/2*-1): # see if that line is clear, i.e., doesn't intersect any walls seg = Segment((robot._gx, robot._gy), (markerX, markerY)) clear = True for w in self.world: if (w.intersects(seg)): # print "in the way", w clear = False break if (clear): # print "I can see ",markerName relXOffM = math.cos(cameraAngleDiff)*distanceToMarker relYOffM = math.sin(cameraAngleDiff)*distanceToMarker foundMarkers.append([relXOffM,relYOffM, markerViewDiff,markerName]) return foundMarkers
def getPosRelativeToKnown(self, knownRowMM, knownColMM, knownAngle, relXOffM, relYOffM, relTh): # print "relXOffM",relXOffM,"relYOffM",relYOffM,"relTh",relTh,\ # "atan",math.atan(relYOffM/relXOffM),"knownAngle",knownAngle toKnownDistMM = calculateHypotenuse(relXOffM * 1000, relYOffM * 1000) toKnownAngle = knownAngle + relTh # print "toKnownAngle",toKnownAngle,"toKnownDistMM",toKnownDistMM,\ # "cos-row",math.cos(toKnownAngle),"sin-col",math.sin(toKnownAngle) # print "rowMMDiff ", (toKnownDistMM * math.cos(toKnownAngle)), \ # "colMMDiff ", (toKnownDistMM * math.sin(toKnownAngle)) relRowMM = knownRowMM - (toKnownDistMM * math.cos(toKnownAngle)) relColMM = knownColMM - (toKnownDistMM * math.sin(toKnownAngle)) relRow = int(round(relRowMM / self.rowScaleMM)) relCol = int(round(relColMM / self.colScaleMM)) return (relRow, relCol, relRowMM, relColMM, (knownAngle + math.pi + relTh) % (math.pi * 2))