def query_sensor(self, position, orientation):
self.make_sure_raytracer_configured()
if self.map is None:
raise BVException('Sensor queried before map was defined.')
if self.sensor_desc is None:
raise BVException('Sensor queried before sensor was defined.')
position = aslist(position)
orientation = asscalar(orientation)
query_object = {"class": "query_sensor",
"position": [position[0], position[1]],
"orientation": orientation}
self.write_to_connection(query_object)
answer = self.child_stream.read_next()
if answer is None:
raise Exception, "Could not communicate with child"
luminance = []
for i, surface_id in enumerate(answer['surface']):
if answer['valid'][i]:
texture = self.surface2texture[surface_id]
coord = answer['curvilinear_coordinate'][i]
luminance.append(asscalar(texture(coord)))
else:
luminance.append(float('nan'))
answer['luminance'] = luminance
return answer
def query_circle(self, center, radius):
""" Returns tuple (hit, surface_id) """
if radius is None or center is None:
raise ValueError('Invalid parameters %s, %s', (center, radius))
radius = asscalar(radius)
centera = array(center, dtype='float32')
if numpy.any(numpy.isnan(centera)) or len(centera) != 2:
raise ValueError('Invalid parameter center: %s ' % center)
if not radius > 0:
raise ValueError('radius must be > 0 (got %s) ' % radius)
center = aslist(center)
self.make_sure_raytracer_configured()
if self.map is None:
raise BVException('query_circle called before map was defined.')
query_object = {"class": "query_circle",
"center": [ center[0], center[1] ],
"radius": radius}
self.write_to_connection(query_object)
answer = self.child_stream.read_next()
if answer is None:
raise BVException, "Could not communicate with child"
assert(answer['class'] == "query_circle_response")
hit = answer['intersects'] == 1
if hit:
surface = answer['surface']
else:
surface = None
return hit, surface
def evolve_state(self, start_state, inputs, dt):
""" Returns the evolved state
commands: array of [-1,1] floats
"""
assert(isinstance(start_state, RigidBodyState))
inputs = aslist(inputs)
expected_num_commands = len(self.commands)
if not (len(inputs) == expected_num_commands):
raise ValueError, 'I was provided %s' % inputs
actual_commands = {}
for i, cmd in enumerate(self.commands):
input = inputs[i]
assert((input >= -1) and (input <= +1))
input01 = (input + 1.0) / 2.0
assert((input01 >= 0) and (input01 <= +1))
value = cmd.min + input01 * (cmd.max - cmd.min)
actual_commands[cmd.id] = value
actual_commands = OpenStruct(**actual_commands)
return self.integrate(start_state, actual_commands, dt)