class WorldInterpretation(unittest.TestCase):
def setUp(self):
self.raytracer = TexturedRaytracer()
self.example_world = {
"class": "map",
"objects": [
{
"class": "polyline", "surface": 0,
"points": [ [-1, -1], [-1, 1], [1, 1], [1, -1], [-1, -1] ],
"texture": "lambda x: numpy.sign(numpy.sin(x))"
},
{
"class": "circle", "surface": 1, "radius": 10, "center": [0, 0],
"texture": 0,
"solid_inside": 0
}
]
}
def testWorldInterpretation(self):
""" Trying to load a canonical example map """
self.assert_(self.raytracer.set_map, self.example_world)
def testArgChecking(self):
""" Trying some invalid inputs """
self.assertRaises(TypeError, self.raytracer.set_map, None)
self.assertRaises(BVException, self.raytracer.set_map, {})
# TODO: write more tests for map format
def testPickling(self):
""" Make sure we can pickle this sensor """
make_sure_pickable(self.raytracer)
def testPickling3(self):
""" Make sure we can pickle this sensor twice """
r2 = make_sure_pickable(self.raytracer)
make_sure_pickable(r2)
def testPickling2(self):
""" Pickling after map loading """
self.raytracer.set_map(create_random_world(10))
make_sure_pickable(self.raytracer)
def testWrongcommand(self):
""" Check an exception is raised if wrong raytracer exec is passed """
raytracer = TexturedRaytracer('raytracer_not')
raytracer.set_map(self.example_world)
self.assertRaises(BVException,
raytracer.query_circle, center=[0, 0], radius=1)
def testIntegrity(self):
""" Make sure that this sensor does not modify the map object """
map = create_random_world(10)
map_original = deepcopy(map)
self.raytracer.set_map(map)
self.assertEqual(map, map_original)
def setup(self, solid_inside):
example_world = {
"class": "map",
"objects": [
{
"class": "circle", "surface": 0, "radius": 1,
"center": [0, 0],
"solid_inside": solid_inside,
"texture": "lambda x: sign(sin(x))"
}
]
}
raytracer = TexturedRaytracer()
raytracer.set_map(example_world)
return raytracer
def setUp(self):
example_world = {
"class": "map",
"objects": [
{
"class": "polyline", "surface": 0,
"points": [ [0, 0], [1, 0]],
"texture": "lambda x: sign(sin(x))"
}
]
}
self.raytracer = TexturedRaytracer()
self.raytracer.set_map(example_world)
class QueryCircleWithSegment(unittest.TestCase):
def setUp(self):
example_world = {
"class": "map",
"objects": [
{
"class": "polyline", "surface": 0,
"points": [ [0, 0], [1, 0]],
"texture": "lambda x: sign(sin(x))"
}
]
}
self.raytracer = TexturedRaytracer()
self.raytracer.set_map(example_world)
# todo, raises if radius < 0
def testRaise(self):
''' check raises if invalid parameters '''
r = self.raytracer
self.assertRaises(ValueError, r.query_circle, [0, 0], -1)
self.assertRaises(ValueError, r.query_circle, [0, 0], 0)
self.assertRaises(ValueError, r.query_circle, [0, 0], float('nan'))
self.assertRaises(ValueError, r.query_circle, [None, 0], 1)
self.assertRaises(ValueError, r.query_circle, [float('nan'), 0], 1)
self.assertRaises(ValueError, r.query_circle, None, 1)
self.assertRaises(ValueError, r.query_circle, 'ciao', 1)
def testIntersection(self):
''' Simple intersections tests '''
surf = 0
r = self.raytracer
eps = 0.0001
self.assertEqual((True, surf), r.query_circle([0, 0], 1))
self.assertEqual((True, surf), r.query_circle([-1, 0], 1 + eps))
self.assertEqual((False, None), r.query_circle([-1, 0], 1 - eps))
self.assertEqual((True, surf), r.query_circle([2, 0], 1 + eps))
self.assertEqual((False, None), r.query_circle([2, 0], 1 - eps))
self.assertEqual((True, surf), r.query_circle([0.5, 0.5], 0.5 + eps))
self.assertEqual((False, None), r.query_circle([0.5, 0.5], 0.5 - eps))
def testSensorSpec1(self):
""" Making sure that the sensor spec survives the pickling """
raytracer = TexturedRaytracer()
raytracer.set_map(example_world)
raytracer.set_sensor(example_sensor)
data1 = raytracer.query_sensor([0, 0], 0)
raytracer2 = make_sure_pickable(raytracer)
data2 = raytracer2.query_sensor([0, 0], 0)
self.assertEqual(data1['readings'], data2['readings'])
def setUp(self):
self.raytracer = TexturedRaytracer()
self.example_world = {
"class": "map",
"objects": [
{
"class": "polyline", "surface": 0,
"points": [ [-1, -1], [-1, 1], [1, 1], [1, -1], [-1, -1] ],
"texture": "lambda x: numpy.sign(numpy.sin(x))"
},
{
"class": "circle", "surface": 1, "radius": 10, "center": [0, 0],
"texture": 0,
"solid_inside": 0
}
]
}
def compute_fields(firstorder_result, world_gen, spacing_xy=1, spacing_theta=90,
resolution=15, previous_result=None):
vehicle = firstorder_result.vehicle
T = firstorder_result.result.T
if previous_result is None:
# Create the lattice for sampling
lattice_x = linspace(-spacing_xy, spacing_xy, resolution)
lattice_y = linspace(-spacing_xy, spacing_xy, resolution)
lattice_theta = linspace(-deg2rad(spacing_theta),
deg2rad(spacing_theta),
resolution)
def make_grid(lattice_row, lattice_col, func):
rows = []
for y in lattice_row:
row = []
for x in lattice_col:
row.append(func(x, y))
rows.append(row)
return rows
result = OpenStruct()
result.lattice_x_y = make_grid(lattice_y, lattice_x,
lambda y, x: RigidBodyState(position=[x, y]))
result.lattice_x_theta = make_grid(lattice_theta, lattice_x,
lambda theta, x: RigidBodyState(position=[x, 0], attitude=theta))
result.lattice_theta_y = make_grid(lattice_y, lattice_theta,
lambda y, theta: RigidBodyState(position=[0, y], attitude=theta))
result.fields_x_y = []
result.fields_x_theta = []
result.fields_theta_y = []
result.worlds = []
result.ref_poses = []
else:
result = previous_result
# This is the number of completed iterations
number_completed = min([ len(x) \
for x in [result.fields_x_y, result.fields_x_theta, result.fields_theta_y]])
print "So far completed %s" % number_completed
# sample world if we don't have one
if len(result.worlds) <= number_completed:
result.worlds.append(world_gen())
world = result.worlds[-1]
# sample pose if we don't have one
if len(result.ref_poses) <= number_completed:
# Initalize raytracer for pose queries
raytracer = TexturedRaytracer()
raytracer.set_map(world)
result.ref_poses.append(
get_safe_pose(raytracer, world_radius=7, # TODO: bounding box
safe_zone=1, num_tries=1000))
del raytracer
ref_pose = result.ref_poses[-1]
vehicle.set_map(world)
num = number_completed * 3
total = (number_completed + 1) * 3
yield (result, num, total)
if len(result.fields_x_y) <= number_completed:
result.fields_x_y.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_x_y))
num += 1
yield (result, num, total)
if len(result.fields_x_theta) <= number_completed:
result.fields_x_theta.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_x_theta))
num += 1
yield (result, num, total)
if len(result.fields_theta_y) <= number_completed:
result.fields_theta_y.append(
compute_command_fields(vehicle, T, ref_pose, result.lattice_theta_y))
num += 1
yield (result, num, total)
def testSensorSpec0(self):
""" Making sure that an exception is thrown if sensor is not specified """
raytracer = TexturedRaytracer()
raytracer.set_map(example_world)
self.assertRaises(BVException, raytracer.query_sensor, [0, 0], 0)
def testWrongcommand(self):
""" Check an exception is raised if wrong raytracer exec is passed """
raytracer = TexturedRaytracer('raytracer_not')
raytracer.set_map(self.example_world)
self.assertRaises(BVException,
raytracer.query_circle, center=[0, 0], radius=1)
def testMapIsReset(self):
""" Making sure that the map is overwritten """
raytracer = TexturedRaytracer()
raytracer.set_sensor(example_sensor)
raytracer.set_map(empty_world)
data1 = raytracer.query_sensor([0, 0], 0)
readings = array(data1['readings'], dtype='float32')
self.assertTrue(isnan(readings).all())
raytracer.set_map(example_world)
data1 = raytracer.query_sensor([0, 0], 0)
readings = array(data1['readings'], dtype='float32')
self.assertTrue(not isnan(readings).any())
raytracer.set_map(empty_world)
data1 = raytracer.query_sensor([0, 0], 0)
readings = array(data1['readings'], dtype='float32')
self.assertTrue(isnan(readings).all())
