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)
