def processSonarReadings(self, grid, robotPose, sonars):
"""
For each reading that is less than the reliable length, set the
point at the end to be occupied and the points along the ray up
to that point to be free.
"""
for (sonarPose, d) in zip(sonarDist.sonarPoses, sonars):
# location of sensor in global frame
s = grid.pointToIndices(robotPose.transformPoint(\
sonarPose.point()))
if d < sonarDist.sonarMax:
# location of sonar 'hit point' in global frame
h = grid.pointToIndices(\
sonarDist.sonarHit(d, sonarPose, robotPose))
# clear list of grid points on the line between the sensor
# and the hit point, not including the hit point
if self.useClearInfo:
for ci in util.lineIndices(s, h)[:-1]:
grid.clearCell(ci)
# Fill in the end point of the reading
grid.setCell(h)
else:
# assume clear if no return (risky)
d = sonarDist.sonarMax
h = grid.pointToIndices(\
sonarDist.sonarHit(d, sonarPose, robotPose))
# clear list of grid points on the line between the sensor
# and the hit point, not including the hit point
if self.useClearInfo:
for ci in util.lineIndices(s, h)[:-1]:
grid.clearCell(ci)
def processSonarReadings(self, grid, robotPose, sonars):
"""
For each reading that is less than the reliable length, set the
point at the end to be occupied and the points along the ray up
to that point to be free.
"""
for (sonarPose, d) in zip(sonarDist.sonarPoses, sonars):
# location of sensor in global frame
s = grid.pointToIndices(robotPose.transformPoint(\
sonarPose.point()))
if d < sonarDist.sonarMax:
# location of sonar 'hit point' in global frame
h = grid.pointToIndices(\
sonarDist.sonarHit(d, sonarPose, robotPose))
# clear list of grid points on the line between the sensor
# and the hit point, not including the hit point
if self.useClearInfo:
for ci in util.lineIndices(s, h)[:-1]:
grid.clearCell(ci)
# Fill in the end point of the reading
grid.setCell(h)
else:
# assume clear if no return (risky)
d = sonarDist.sonarMax
h = grid.pointToIndices(\
sonarDist.sonarHit(d, sonarPose, robotPose))
# clear list of grid points on the line between the sensor
# and the hit point, not including the hit point
if self.useClearInfo:
for ci in util.lineIndices(s, h)[:-1]:
grid.clearCell(ci)
def idealSonarReading(robotPose, sensorPose, world):
"""
@param robotPose: C{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: C{soarWorld.SoarWorld} representing obstacles in the world
@returns: length of ideal sonar reading; if the distance is
longer than C{sonarDist.sonarMax} or there is no hit at all, then
C{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)
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)
