def cairo_plot_sensor_skins(cr, vehicle_state, scale=1.0):
for attached in vehicle_state['sensors']:
sensor = attached['sensor']
sensor_pose = SE2_from_SE3(SE3.from_yaml(attached['current_pose']))
extra = attached.get('extra', {})
sensor_skin = extra.get('skin', None)
if sensor_skin is None:
sensor_skin = sensor['type']
skins_library = get_conftools_skins()
if not sensor_skin in skins_library:
logger.warning('Could not find skin %r' % sensor_skin)
else:
skin = skins_library.instance(sensor_skin)
with cairo_rototranslate(cr, sensor_pose):
cr.scale(scale, scale)
skin.draw(cr)
def draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not(0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
def draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not (0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
# function(*arguments)
# except:
# msg = ('Error detected when running test (%s); displaying debug info.'
# % function.__name__)
# if dynamics is not None:
# msg += '\ndynamics: %s' % dynamics
# # TODO: write other info
# logger.error(msg)
# raise
for_all_dynamics = comptests_for_all(get_conftools_dynamics())
for_all_vehicles = comptests_for_all(get_conftools_vehicles())
for_all_worlds = comptests_for_all(get_conftools_worlds())
for_all_sensors = comptests_for_all(get_conftools_sensors())
for_all_skins = comptests_for_all(get_conftools_skins())
for_all_world_vehicle_pairs = comptests_for_all_pairs(get_conftools_worlds(),
get_conftools_vehicles())
#
# for_all_dynamics = fancy_test_decorator(lister=all_dynamics,
# arguments=lambda x: (x, get_dynamics(x)),
# attributes=lambda x: dict(dynamics=x))
#
# for_all_worlds = fancy_test_decorator(lister=all_worlds,
# arguments=lambda x: (x, get_world(x)),
# attributes=lambda x: dict(world=x))
#
# for_all_vehicles = fancy_test_decorator(lister=all_vehicles,
# arguments=lambda x: (x, get_vehicle(x)),
# attributes=lambda x: dict(vehicle=x))
# try:
# function(*arguments)
# except:
# msg = ('Error detected when running test (%s); displaying debug info.'
# % function.__name__)
# if dynamics is not None:
# msg += '\ndynamics: %s' % dynamics
# # TODO: write other info
# logger.error(msg)
# raise
for_all_dynamics = comptests_for_all(get_conftools_dynamics())
for_all_vehicles = comptests_for_all(get_conftools_vehicles())
for_all_worlds = comptests_for_all(get_conftools_worlds())
for_all_sensors = comptests_for_all(get_conftools_sensors())
for_all_skins = comptests_for_all(get_conftools_skins())
for_all_world_vehicle_pairs = comptests_for_all_pairs(get_conftools_worlds(),
get_conftools_vehicles())
#
# for_all_dynamics = fancy_test_decorator(lister=all_dynamics,
# arguments=lambda x: (x, get_dynamics(x)),
# attributes=lambda x: dict(dynamics=x))
#
# for_all_worlds = fancy_test_decorator(lister=all_worlds,
# arguments=lambda x: (x, get_world(x)),
# attributes=lambda x: dict(world=x))
#
# for_all_vehicles = fancy_test_decorator(lister=all_vehicles,
# arguments=lambda x: (x, get_vehicle(x)),
# attributes=lambda x: dict(vehicle=x))
def vehicles_cairo_display_all(cr, width, height,
sim_state,
grid=1,
zoom=0,
bgcolor=[1, 1, 1],
zoom_scale_radius=False,
extra_draw_world=None,
first_person=True,
show_world=True,
# show_features='relevant',
show_sensor_data=True,
show_sensor_data_compact=False,
show_robot_body=True,
show_robot_sensors=True,
show_textures=True,
plot_sources=False):
'''
:param zoom_scale_radius: If true, scales the zoom by the robot radius.
'''
with cairo_save(cr):
# Paint white
if bgcolor is not None:
with cairo_save(cr):
cairo_set_color(cr, bgcolor)
cr.rectangle(0, 0, width, height)
cr.fill()
vehicle_state = sim_state['vehicle']
robot_pose = SE2_from_SE3(SE3.from_yaml(vehicle_state['pose']))
robot_radius = vehicle_state['radius']
world_state = sim_state['world']
bounds = world_state['bounds']
bx = bounds[0]
by = bounds[1]
if zoom_scale_radius and zoom != 0:
zoom = zoom * robot_radius
world_bounds_pad = 0 # CairoConstants.texture_width
vehicles_cairo_set_coordinates(cr, width, height,
bounds, robot_pose,
zoom=zoom,
world_bounds_pad=world_bounds_pad,
first_person=first_person)
if False:
cr.set_source_rgb(0, 1, 0)
cr.set_line_width(0.05)
cr.rectangle(bx[0], by[0], bx[1] - bx[0], by[1] - by[0])
cr.stroke()
if grid > 0:
show_grid(cr, bx, by, spacing=grid, margin=1)
if extra_draw_world is not None:
extra_draw_world(cr)
sensor_types = get_sensor_types(vehicle_state)
has_cameras = (VehiclesConstants.SENSOR_TYPE_PHOTORECEPTORS
in sensor_types)
has_field_sampler = (VehiclesConstants.SENSOR_TYPE_FIELDSAMPLER
in sensor_types)
if show_world:
if has_field_sampler:
cairo_plot_sources(cr, world_state)
plot_textures = has_cameras and show_textures
cairo_show_world_geometry(cr, world_state,
plot_textures=plot_textures,
plot_sources=plot_sources,
extra_pad=world_bounds_pad)
# XXX: tmp
if extra_draw_world is not None:
extra_draw_world(cr)
if show_robot_body:
joints = get_joints_as_TSE3(vehicle_state)
extra = vehicle_state.get('extra', {})
id_skin = extra.get('skin', 'default_skin')
skin = get_conftools_skins().instance(id_skin)
with cairo_rototranslate(cr, robot_pose):
cr.scale(robot_radius, robot_radius)
skin.draw(cr, joints=joints,
timestamp=sim_state['timestamp'])
if show_robot_sensors:
# don't like it cause it uses global "current_pose"
cairo_plot_sensor_skins(cr, vehicle_state,
scale=robot_radius)
if show_sensor_data:
cairo_plot_sensor_data(cr, vehicle_state,
scale=robot_radius,
compact=show_sensor_data_compact)
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)