def testFF():
sonars = [0.0] * 8
poseList = [
util.Pose(0, 0, 0),
util.Pose(0, 1, 0),
util.Pose(0.499, 0.501, 2),
util.Pose(2, 3, 4)
]
ffTestInput = [io.FakeSensorInput(sonars, pose) for pose in poseList]
targetGenerator = ffSkeleton.FollowFigure(points)
result = targetGenerator.transduce(ffTestInput, check=True)
print 'The actual inputs are whole instances of io.SensorInput; here we'
print 'are just showing the odometry part of the input.'
for (i, o) in zip(poseList, result):
print '\nInput:', i
print 'Output:', o
def testMove():
target = util.Point(1.0, 0.5)
sonars = [0.0] * 8
poseList = [
util.Pose(0, 0, 0),
util.Pose(0, 0, math.pi / 2),
util.Pose(0, 0, math.atan2(0.5, 1)),
util.Pose(1.0001, 0.499999, 0)
]
moveTestInput = [(target, io.FakeSensorInput(sonars, pose)) \
for pose in poseList]
mover = dynamicMoveToPointSkeleton.DynamicMoveToPoint()
result = mover.transduce(moveTestInput, check=True)
print 'The actual inputs are (target, sensorInput) pairs; here we'
print 'are just showing the odometry part of the sensorInput.'
for ((goal, sensors), o) in zip(moveTestInput, result):
print '\nInput:', (goal, sensors.odometry)
print 'Output:', o
import math
import lib601.ucSearch as ucSearch
import lib601.util as util
import lib601.basicGridMap as basicGridMap
import lib601.gridMap as gridMap
import lib601.sm as sm
######################################################################
### Picking worlds
######################################################################
mapTestWorld = ['mapTestWorld.py', 0.2, util.Point(2.0, 5.5),
util.Pose(2.0, 0.5, 0.0)]
bigPlanWorld = ['bigPlanWorld.py', 0.25, util.Point(3.0, 1.0),
util.Pose(1.0, 1.0, 0.0)]
class GridDynamics(sm.SM):
legalInputs = ['u','ur','r','dr','d','dl','l','ul']
def __init__(self, theMap):
pass
def nextState(self,state,inp):
nextState = list(state)
if inp == 'u':
nextState[1] += 1
cost = 1.
elif inp == 'ur':
nextState[0] += 1
nextState[1] += 1
cost = 2.**(0.5)
elif inp == 'r':
self.startState = None # change this
def getNextValues(self, state, inp):
pass
# For testing your map maker
class SensorInput:
def __init__(self, sonars, odometry):
self.sonars = sonars
self.odometry = odometry
testData = [
SensorInput([0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2],
util.Pose(1.0, 2.0, 0.0)),
SensorInput([0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4],
util.Pose(4.0, 2.0, -math.pi))
]
testClearData = [
SensorInput([1.0, 5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.0],
util.Pose(1.0, 2.0, 0.0)),
SensorInput([1.0, 5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.0],
util.Pose(4.0, 2.0, -math.pi))
]
def testMapMaker(data):
(xMin, xMax, yMin, yMax, gridSquareSize) = (0, 5, 0, 5, 0.1)
mapper = MapMaker(xMin, xMax, yMin, yMax, gridSquareSize)
"""
Useful constants and utilities for dealing with sonar readings in soar.
"""
import math
import lib601.util as util
sonar_poses = [
util.Pose(0.08, 0.134, math.pi / 2),
util.Pose(0.122, 0.118, 5 * math.pi / 18),
util.Pose(0.156, 0.077, math.pi / 6),
util.Pose(0.174, 0.0266, math.pi / 18),
util.Pose(0.174, -0.0266, -math.pi / 18),
util.Pose(0.156, -0.077, -math.pi / 6),
util.Pose(0.122, -0.118, -5 * math.pi / 18),
util.Pose(0.08, -0.134, -math.pi / 2)
]
"""Positions and orientations of sonar sensors with respect to the
center of the robot."""
sonar_max = 1.5
"""Maximum good sonar reading."""
def get_distance_right(sonar_values):
"""
@param sonar_values: list of 8 sonar readings
@return: the perpendicular distance to a surface on the right of
the robot, assuming there is a linear surface.
"""
return get_distance_right_and_angle(sonar_values)[0]
import lib601.ssm as ssm
import lib601.sonarDist as sonarDist
import lib601.move as move
import lib601.seGraphics as seGraphics
import lib601.idealReadings as idealReadings
# For testing your preprocessor
class SensorInput:
def __init__(self, sonars, odometry):
self.sonars = sonars
self.odometry = odometry
preProcessTestData = [
SensorInput([0.8, 1.0], util.Pose(1.0, 0.5, 0.0)),
SensorInput([0.25, 1.2], util.Pose(2.4, 0.5, 0.0)),
SensorInput([0.16, 0.2], util.Pose(7.3, 0.5, 0.0))
]
testIdealReadings = (5, 1, 1, 5, 1, 1, 1, 5, 1, 5)
testIdealReadings100 = (50, 10, 10, 50, 10, 10, 10, 50, 10, 50)
class PreProcess(sm.SM):
def __init__(self, numObservations, stateWidth):
self.startState = (None, None)
self.numObservations = numObservations
self.stateWidth = stateWidth
def getNextValues(self, state, inp):
(lastUpdatePose, lastUpdateSonar) = state