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 determineMove(self, subgoal, sonar): # note that we assume that the row and col given are subgoals, this # algorithm can NOT deal with walls or obstacles. subgoalRow, subgoalCol = subgoal subgoalXmm = subgoalRow * self.rowScaleMM subgoalYmm = subgoalCol * self.colScaleMM angleToGoal = math.atan2((subgoalYmm - self.y_localizeMM) * -1, (subgoalXmm - self.x_localizeMM) * -1) robotAngleDiff = -normalizeAngle2(angleToGoal - normalizeAngle2(self.thr_localize)) print "DIFF: ", robotAngleDiff sensores = sonar # sensores = [2.0*float(sonar[k])/float(max(sonar)) for k in range(len(sonar))] velgiro = self.calculadorNeuronal.transferencia(sensores + [robotAngleDiff / (2.0 * math.pi)]) velavan = self.calculaVelocidadAvance(velgiro) return (velavan, velgiro)