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 get_observations(self):
if not self.boot_episode_started:
raise Exception('get_observations() called before new_episode().')
observations = VehicleSimulation.compute_observations(self)
episode_end = True if self.vehicle_collided else False
if episode_end:
self.info("Ending boot episode due to collision.")
self.boot_episode_started = False
return RobotObservations(timestamp=self.timestamp,
observations=observations,
commands=self.last_commands,
commands_source=self.commands_source,
robot_pose=self.vehicle.get_pose(),
episode_end=episode_end)
def get_observations(self):
if not self.boot_episode_started:
raise Exception('get_observations() called before new_episode().')
observations = VehicleSimulation.compute_observations(self)
episode_end = True if self.vehicle_collided else False
if episode_end:
self.info("Ending boot episode due to collision.")
self.boot_episode_started = False
return RobotObservations(timestamp=self.timestamp,
observations=observations,
commands=self.last_commands,
commands_source=self.commands_source,
robot_pose=self.vehicle.get_pose(),
episode_end=episode_end)
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)
def run_simulation(task, vehicle, agent, log, dt, maxT):
world = task.get_world()
simulation = VehicleSimulation(world=world, vehicle=vehicle)
directions = simulation.vehicle.sensors[0].sensor.directions
vehicle_spec = VehicleSpec(directions=directions)
agent.init(vehicle_spec)
simulation.new_episode()
tmp_log = log + '.active'
ldir = os.path.dirname(log)
#last = os.path.join(ldir, 'last.yaml')
logger.info('Writing on log %r.' % log)
#logger.info(' (also accessible as %r)' % last)
if not os.path.exists(ldir):
os.makedirs(ldir)
#if os.path.exists(last):
# os.unlink(last)
logfile = open(tmp_log, 'w')
#assert not os.path.exists(last)
assert os.path.exists(tmp_log)
#logger.info('Link %s, %s' % (tmp_log, last))
#os.symlink(tmp_log, last)
logger.info('Simulation dt=%.3f max T: %.3f' % (dt, maxT))
while True:
simulation.compute_observations()
# TODO: perhaps this needs to be gerealized
luminance = simulation.vehicle.sensors[0].current_observations['luminance']
observations = VehicleObservations(time=simulation.timestamp,
dt=dt,
luminance=luminance)
agent.process_observations(observations)
commands = agent.choose_commands()
# TODO: check format
if logfile is not None:
y = simulation.to_yaml()
y['commands'] = commands.tolist()
logfile.write('---\n')
yaml.dump(y, logfile, Dumper=Dumper)
logger.info('t=%.3f pose: %s' %
(simulation.timestamp,
SE2.friendly(SE2_from_SE3(simulation.vehicle.get_pose()))))
if task.end_condition(simulation):
break
if simulation.timestamp > maxT:
# TODO: add why we finished
break
simulation.simulate(commands, dt)
logfile.close()
if os.path.exists(log):
os.unlink(log)
assert not os.path.exists(log)
os.rename(tmp_log, log)
assert not os.path.exists(tmp_log)
assert os.path.exists(log)
