Esempio n. 1
0
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)
Esempio n. 2
0
    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)
Esempio n. 3
0
    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)
Esempio n. 4
0
#         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))
Esempio n. 5
0
#     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))
Esempio n. 6
0
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)
Esempio n. 7
0
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)
Esempio n. 8
0
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)