def set_world_primitives(self, primitives):
# FIXME this does not work with changing environments
if primitives: # FIXME: only find changed things
sources = [p for p in primitives if isinstance(p, Source)]
if not sources:
logger.error('Warning: there no sources given for a field '
'sampler in this world.')
if self.primitives is None:
self.primitives = primitives
def check_simulation_one_step_conf(id_world, world, id_vehicle, vehicle):
sim = VehicleSimulation(vehicle, world)
dt = 1
try:
sim.new_episode()
cmds = random_commands(sim.vehicle.dynamics.get_commands_spec())
sim.simulate(cmds, dt)
sim.compute_observations()
except:
logger.error('Error for vehicle=%s world=%s' % (id_vehicle, id_world))
raise
def instance_vehicle_spec(entry):
from ..simulation import Vehicle
from ..interfaces import VehicleSensor, Dynamics
from vehicles.configuration.master import get_conftools_dynamics, \
get_conftools_sensors
check_valid_vehicle_config(entry)
try:
master = get_conftools_dynamics()
if 'id_dynamics' in entry:
id_dynamics = entry['id_dynamics']
dynamics = master.instance(id_dynamics)
else:
id_dynamics = entry['dynamics']['id']
dynamics = master.instance_spec(entry['dynamics'])
assert isinstance(dynamics, Dynamics)
sensors = entry['sensors']
radius = entry['radius']
extra = entry.get('extra', {})
vehicle = Vehicle(radius=radius, extra=extra)
vehicle.add_dynamics(id_dynamics, dynamics)
master = get_conftools_sensors()
for sensor in sensors:
if 'id_sensor' in sensor:
id_sensor = sensor['id_sensor']
sensor_instance = master.instance(id_sensor)
else:
id_sensor = sensor['sensor']['id']
sensor_instance = master.instance_spec(sensor['sensor'])
assert isinstance(sensor_instance, VehicleSensor)
# TODO: document this
x, y, theta_deg = sensor.get('pose', [0, 0, 0])
theta = np.radians(theta_deg)
pose = SE2_from_translation_angle([x, y], theta)
pose = SE3_from_SE2(pose)
joint = sensor.get('joint', 0)
extra = sensor.get('extra', {})
vehicle.add_sensor(id_sensor=id_sensor,
sensor=sensor_instance,
pose=pose, joint=joint,
extra=extra)
return vehicle
except:
logger.error('Error while trying to instantiate vehicle. Entry:\n%s'
% (pformat(entry)))
raise
def compute_observations(self, pose):
""" Computes the observations for this vehicle
Args:
pose: SE2 (world reference frame.)
Returns:
a dictionary, containing as many data fields as desired. This data
is eventually passed to the client as a OpenStruct.
There is, however, one necessary field.
* data[SENSELS] should be the declared sensel for the sensor.
This should be a numpy.ndarray, or a list of numbers.
No NANs allowed.
"""
observations = self._compute_observations(pose)
if not isinstance(observations, dict):
raise ValueError('This should return a dict()')
if not VehicleSensor.SENSELS in observations:
raise ValueError('No field %r in %r' %
(VehicleSensor.SENSELS, observations.keys()))
sensels = observations[VehicleSensor.SENSELS]
sensels = np.array(sensels)
if DO_EXTRA_CHECKS:
# todo: check spec
# check("array[K]", sensels,
# desc='I expect a unidimenional array/list for sensels.')
try:
notfinite = not np.isfinite(sensels).all()
except Exception as e:
logger.error('Error is: %s' % e) # XXX: print
logger.error('Data is: %s' % sensels.dtype)
logger.error((observations))
notfinite = False
if notfinite:
msg = ('Not all are valid:\n%s\n%s' %
(sensels, pformat(observations)))
logger.error(msg)
logger.error('pose: %s' % pose) # XXX
# XXX: - we will solve this later.
sensels[np.logical_not(np.isfinite(sensels))] = 0.5
raise ValueError(msg)
# XXX: maybe somewhere else?
# if sensels.size != self.num_sensels:
# raise ValueError('I was expecting %d sensels, not %s.' %
# (self.num_sensels, sensels.size))
observations[VehicleSensor.SENSELS] = sensels
return observations
def integrate(self, state, commands, dt):
# self._state_space.belongs(state)
self.check_commands(commands)
try:
new_state = self._integrate(state, commands, dt)
except:
msg = 'Error while integrating.'
try:
msg += '\n state: %s' % self._state_space.friendly(state)
except: # XXX
msg += '\n state: %s' % state.__repr__()
msg += '\ncommands: %s' % commands.__repr__()
msg += '\n%s' % traceback.format_exc()
logger.error(msg)
raise
# self._state_space.belongs(new_state)
return new_state
''' Code interfacing PyVehicles with BootstrappingOlympics. '''
from vehicles import __version__, logger
try:
import bootstrapping_olympics
vehicles_has_boot_olympics = True
except ImportError:
logger.error('Package BootOlympics not installed, vehicles_boot cannot '
'work properly.')
vehicles_has_boot_olympics = False
if vehicles_has_boot_olympics:
from .vehicles_simulation import *
from .ros_visualization import *
def get_comptests():
# get testing configuration directory
from pkg_resources import resource_filename # @UnresolvedImport
dirname = resource_filename("vehicles_boot", "configs")
from vehicles import get_vehicles_config
get_vehicles_config().load('default')
# load into bootstrapping_olympics
from comptests import get_comptests_app
from bootstrapping_olympics import get_boot_config
boot_config = get_boot_config()
boot_config.load(dirname)
# Our tests are its tests with our configuration
def main():
if not vehicles_has_cairo:
logger.error('This program cannot be run if Cairo is not installed.')
return
from vehicles_cairo import (cairo_plot_rectangle)
from vehicles_cairo import cairo_plot_circle, cairo_set_axis, show_grid
parser = OptionParser(usage=usage)
parser.disable_interspersed_args()
parser.add_option("--outdir", "-o", default='.',
help="output directory [%default]")
# parser.add_option("--outdir", "-o", default='.',
# help="additional config directory [%default]")
(options, args) = parser.parse_args()
if args:
raise Exception()
width = 400
height = 400
r = Report('skins_demo')
f = r.figure(cols=3)
import cairo
VehiclesConfig.make_sure_loaded()
skins = get_conftools_skins().keys()
logger.info('Skins: %s' % skins)
for id_skin in skins:
skin = get_conftools_skins().instance(id_skin)
with f.data_file(id_skin, MIME_PNG) as filename:
surf = cairo.ImageSurface(cairo.FORMAT_ARGB32, # @UndefinedVariable
width, height)
cr = cairo.Context(surf) # @UndefinedVariable
cairo_plot_rectangle(cr, 0, 0, width, height, fill_color=[1, 1, 1])
cairo_set_axis(cr, width, height, [-2, +2, -2, +2])
cairo_plot_circle(cr, center=[0, 0], radius=1, facecolor=None,
edgecolor=(1, 0, 0), width=0.1)
show_grid(cr, bx=[-3, +3], by=[-3, +3], spacing=1.0)
if skin.njoints_required() > 1:
logger.warning('Skipping skin %r' % id_skin)
continue
try:
skin.draw(cr, joints=[])
except Exception:
logger.error('Error for skin %r' % id_skin)
raise
surf.write_to_png(filename) # Output to PNG
filename = os.path.join(options.outdir, 'index.html')
logger.info('Writing to %r.' % filename)
r.to_html(filename)
def main():
if not vehicles_has_cairo:
logger.error('This program cannot be run if Cairo is not installed.')
return
from vehicles_cairo import (cairo_plot_rectangle)
from vehicles_cairo import cairo_plot_circle, cairo_set_axis, show_grid
parser = OptionParser(usage=usage)
parser.disable_interspersed_args()
parser.add_option("--outdir",
"-o",
default='.',
help="output directory [%default]")
# parser.add_option("--outdir", "-o", default='.',
# help="additional config directory [%default]")
(options, args) = parser.parse_args()
if args:
raise Exception()
width = 400
height = 400
r = Report('skins_demo')
f = r.figure(cols=3)
import cairo
VehiclesConfig.make_sure_loaded()
skins = get_conftools_skins().keys()
logger.info('Skins: %s' % skins)
for id_skin in skins:
skin = get_conftools_skins().instance(id_skin)
with f.data_file(id_skin, MIME_PNG) as filename:
surf = cairo.ImageSurface(
cairo.FORMAT_ARGB32, # @UndefinedVariable
width,
height)
cr = cairo.Context(surf) # @UndefinedVariable
cairo_plot_rectangle(cr, 0, 0, width, height, fill_color=[1, 1, 1])
cairo_set_axis(cr, width, height, [-2, +2, -2, +2])
cairo_plot_circle(cr,
center=[0, 0],
radius=1,
facecolor=None,
edgecolor=(1, 0, 0),
width=0.1)
show_grid(cr, bx=[-3, +3], by=[-3, +3], spacing=1.0)
if skin.njoints_required() > 1:
logger.warning('Skipping skin %r' % id_skin)
continue
try:
skin.draw(cr, joints=[])
except Exception:
logger.error('Error for skin %r' % id_skin)
raise
surf.write_to_png(filename) # Output to PNG
filename = os.path.join(options.outdir, 'index.html')
logger.info('Writing to %r.' % filename)
r.to_html(filename)
def main():
from vehicles_cairo import vehicles_has_cairo
if not vehicles_has_cairo:
logger.error('This program cannot be run if Cairo is not installed.')
return
from vehicles_cairo import (vehicles_cairo_display_pdf,
vehicles_cairo_display_png,
vehicles_cairo_display_svg,
cairo_plot_circle2)
parser = OptionParser(usage=usage)
parser.disable_interspersed_args()
parser.add_option("--vehicles", default='*',
help="ID vehicle [%default].")
parser.add_option("--world", default='SBox2_10a',
# default='empty_fixed',
help="ID world [%default].")
parser.add_option("--outdir", "-o",
default='vehicles_demo_display_vehicles',
help="output directory [%default]")
parser.add_option("--figsize", default=10, type='float',
help="figsize (inches) [%default]")
parser.add_option("-g", "--grid", default=1, type='float',
help="grid size in meters; 0 for no grid [%default]")
parser.add_option("-d", dest="config", default=".",
help="Config directory")
parser.add_option("--scale", default=False, action='store_true',
help="If given, displays the scale with a red circle")
(options, args) = parser.parse_args()
if args:
raise Exception() # XXX
id_world = options.world
logger.info(' id_world: %s' % id_world)
from reprep import Report, MIME_PDF
basename = 'vehicles_demo'
r = Report(basename)
logger.info('Loading configuration from %s' % options.config)
VehiclesConfig.load(options.config)
# TODO: selection
all_vehicles = VehiclesConfig.vehicles.keys()
if options.vehicles is None:
vehicles = all_vehicles
else:
vehicles = expand_string(options.vehicles, all_vehicles)
print('Plotting vehicles: %s' % vehicles)
f0 = r.figure(cols=6)
f0_data = r.figure(cols=6)
for id_vehicle in sorted(vehicles):
sec = r.node(id_vehicle)
f = sec.figure(cols=6)
world = VehiclesConfig.specs['worlds'].instance(id_world)
vehicle = VehiclesConfig.specs['vehicles'].instance(id_vehicle)
simulation = VehicleSimulation(vehicle, world)
simulation.new_episode()
simulation.compute_observations()
sim_state = simulation.to_yaml()
def draw_scale(cr):
if options.scale:
cairo_plot_circle2(cr, 0, 0,
vehicle.radius, fill_color=(1, .7, .7))
plot_params = dict(grid=options.grid,
zoom=1.5,
zoom_scale_radius=True,
width=500, height=500,
show_sensor_data=True,
show_sensor_data_compact=True,
extra_draw_world=draw_scale,
bgcolor=None,
show_world=False)
with f.data_file('png_with', MIME_PNG) as filename:
vehicles_cairo_display_png(filename,
sim_state=sim_state, **plot_params)
f0_data.sub(f.last(), caption=id_vehicle)
plot_params['show_sensor_data'] = False
with f.data_file('png_without', MIME_PNG) as filename:
vehicles_cairo_display_png(filename,
sim_state=sim_state, **plot_params)
f0.sub(f.last(), caption=id_vehicle)
with f.data_file('svg', MIME_SVG) as filename:
vehicles_cairo_display_svg(filename,
sim_state=sim_state, **plot_params)
plot_params['grid'] = 0
plot_params['extra_draw_world'] = None
with sec.data_file('pdf', MIME_PDF) as filename:
vehicles_cairo_display_pdf(filename,
sim_state=sim_state, **plot_params)
plot_params['show_robot_body'] = True
plot_params['show_robot_sensors'] = False
with f.data_file('png_only_body', MIME_PNG) as filename:
vehicles_cairo_display_png(filename,
sim_state=sim_state, **plot_params)
filename = os.path.join(options.outdir, 'index.html')
logger.info('Writing to %r.' % filename)
r.to_html(filename)
''' Code interfacing PyVehicles with BootstrappingOlympics. '''
from vehicles import __version__, logger
try:
import bootstrapping_olympics
vehicles_has_boot_olympics = True
except ImportError:
logger.error('Package BootOlympics not installed, vehicles_boot cannot '
'work properly.')
vehicles_has_boot_olympics = False
if vehicles_has_boot_olympics:
from .vehicles_simulation import *
from .ros_visualization import *
def get_comptests():
# get testing configuration directory
from pkg_resources import resource_filename # @UnresolvedImport
dirname = resource_filename("vehicles_boot", "configs")
from vehicles import get_vehicles_config
get_vehicles_config().load('default')
# load into bootstrapping_olympics
from comptests import get_comptests_app
from bootstrapping_olympics import get_boot_config
boot_config = get_boot_config()
boot_config.load(dirname)
