def indicesToBoxSegs(self, indices):
"""
@param indices: pair of C{(ix, iy)} indices of a grid cell
@returns: list of four line segments that constitute the
boundary of the cell, grown by the radius of the robot, which
is found in C{gridMap.robotRadius}.
"""
center = self.indicesToPoint(indices)
xs = self.xStep/2 + gridMap.robotRadius
ys = self.yStep/2 + gridMap.robotRadius
vertices = [center + util.Point(xs, ys),
center + util.Point(xs, -ys),
center + util.Point(-xs, -ys),
center + util.Point(-xs, ys)]
segs = [util.LineSeg(vertices[0], vertices[1]),
util.LineSeg(vertices[1], vertices[2]),
util.LineSeg(vertices[2], vertices[3]),
util.LineSeg(vertices[3], vertices[0])]
return segs
def idealSonarReading(robotPose, sensorPose, world):
"""
:param robotPose: ``util.Pose`` representing pose of robot in world
:param sensorPose: c{util.Pose} representing pose of sonar sensor
with respect to the robot
:param world: ``soarWorld.SoarWorld`` representing obstacles in the world
:returns: length of ideal sonar reading; if the distance is
longer than ``sonarDist.sonarMax`` or there is no hit at all, then
``sonarDist.sonarMax`` is returned.
"""
sensorOriginPoint = sonarDist.sonarHit(0, sensorPose, robotPose)
sonarRay = util.LineSeg(sensorOriginPoint,
sonarDist.sonarHit(sonarDist.sonarMax,
sensorPose, robotPose))
hits = [(seg.intersection(sonarRay), seg) for seg in world.wallSegs]
distances = [sensorOriginPoint.distance(hit) for (hit,seg) in hits if hit]
distances.append(sonarDist.sonarMax)
return min(distances)
class SoarWorld:
"""
Represents a world in the same way as the soar simulator
"""
def __init__(self, path):
"""
@param path: String representing location of world file
"""
self.walls = []
"""
Walls represented as list of pairs of endpoints
"""
self.wallSegs = []
"""
Walls represented as list of C{util.lineSeg}
"""
# set the global world
global world
world = self
# execute the file for side effect on world
execfile(path)
# put in the boundary walls
(dx, dy) = self.dimensions
wall((0, 0), (0, dy))
wall((0, 0), (dx, 0))
wall((dx, 0), (dx, dy))
wall((0, dy), (dx, dy))
def initialLoc(self, x, y):
# Initial robot location
self.initialRobotLoc = util.Point(x, y)
def dims(self, dx, dy):
# x and y dimensions
self.dimensions = (dx, dy)
def addWall(self, (xlo, ylo), (xhi, yhi)):
# walls are defined by two points
self.walls.append((util.Point(xlo, ylo), util.Point(xhi, yhi)))
# also store representation as line segments
self.wallSegs.append(
util.LineSeg(util.Point(xlo, ylo), util.Point(xhi, yhi)))