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)