def cairo_show_world_geometry(cr, world_state, plot_sources=False, plot_textures=False, extra_pad=0): bounds = world_state['bounds'] primitives = world_state['primitives'] with cairo_save(cr): xb = bounds[0] yb = bounds[1] xb = [xb[0] - extra_pad, xb[1] + extra_pad] yb = [yb[0] - extra_pad, yb[1] + extra_pad] cr.rectangle(xb[0], yb[0], xb[1] - xb[0], yb[1] - yb[0]) cr.clip() # Draw it twice for special effects for i in range(2): for p in primitives: ptype = p['type'] if ptype == VehiclesConstants.PRIMITIVE_POLYLINE: points = np.array(p['points']).T texture = instantiate_spec(p['texture']) if not plot_textures: texture = None cairo_plot_polyline_primitive(cr, points=points, texture=texture) elif ptype == VehiclesConstants.PRIMITIVE_CIRCLE: texture = instantiate_spec(p['texture']) if not plot_textures: texture = None cairo_plot_circle_primitive(cr, center=p['center'], texture=texture, radius=p['radius'], solid=p['solid'], numpass=i) elif ptype == VehiclesConstants.PRIMITIVE_SOURCE: if plot_sources: cairo_plot_circle(cr, center=p['center'], radius=0.05, edgecolor=[0, 0, 0], facecolor=[1, 0, 0], width=0.01) # TODO: parametrize else: pass # XXX
def plot_fieldsampler(cr, pose, positions, sensels, radius=None): # Scale in [0,1] for better contrast sensels = sensels - np.min(sensels) if np.max(sensels) > 0: sensels = sensels / np.max(sensels) # find radius if none given as the minimum distance of the first # sensels to the others if radius is None: radius = find_radius(positions) for i, value in enumerate(sensels.flat): value = 1 - value facecolor = [value, value, value] pw = SE2_act_R2(pose, positions[i, :]) # alpha=0.6 cairo_plot_circle(cr, center=pw, radius=radius, facecolor=facecolor, edgecolor=None)
def cairo_show_world_geometry(cr, world_state, plot_sources=False, plot_textures=False, extra_pad=0): bounds = world_state["bounds"] primitives = world_state["primitives"] with cairo_save(cr): xb = bounds[0] yb = bounds[1] xb = [xb[0] - extra_pad, xb[1] + extra_pad] yb = [yb[0] - extra_pad, yb[1] + extra_pad] cr.rectangle(xb[0], yb[0], xb[1] - xb[0], yb[1] - yb[0]) cr.clip() # Draw it twice for special effects for i in range(2): for p in primitives: ptype = p["type"] if ptype == VehiclesConstants.PRIMITIVE_POLYLINE: points = np.array(p["points"]).T texture = instantiate_spec(p["texture"]) if not plot_textures: texture = None cairo_plot_polyline_primitive(cr, points=points, texture=texture) elif ptype == VehiclesConstants.PRIMITIVE_CIRCLE: texture = instantiate_spec(p["texture"]) if not plot_textures: texture = None cairo_plot_circle_primitive( cr, center=p["center"], texture=texture, radius=p["radius"], solid=p["solid"], numpass=i ) elif ptype == VehiclesConstants.PRIMITIVE_SOURCE: if plot_sources: cairo_plot_circle( cr, center=p["center"], radius=0.05, edgecolor=[0, 0, 0], facecolor=[1, 0, 0], width=0.01 ) # TODO: parametrize else: pass # XXX
def cairo_plot_circle_primitive(cr, center, radius, solid, texture, numpass): facecolor = (CairoConstants.obstacle_fill_color if solid else None) if numpass == 0: cairo_plot_circle(cr, center=center, radius=radius, edgecolor=CC.obstacle_border_color, facecolor=facecolor, width=CC.obstacle_border_width) if not solid: cairo_plot_circle(cr, center=center, radius=radius + CC.texture_width, edgecolor=[0, 0, 0], width=CC.obstacle_border_width) if texture is not None: if solid: width_outside = 0 width_inside = CC.texture_width else: width_inside = 0 width_outside = CC.texture_width cairo_plot_texture_circle(cr=cr, radius=radius, center=center, texture=texture, resolution=CC.texture_resolution, width_inside=width_inside, width_outside=width_outside) else: r1 = radius - CC.texture_width if r1 > 0: cairo_plot_circle(cr, center=center, radius=r1, edgecolor=None, facecolor=facecolor, width=CC.obstacle_border_width)
def cairo_plot_circle_primitive(cr, center, radius, solid, texture, numpass): facecolor = CairoConstants.obstacle_fill_color if solid else None if numpass == 0: cairo_plot_circle( cr, center=center, radius=radius, edgecolor=CC.obstacle_border_color, facecolor=facecolor, width=CC.obstacle_border_width, ) if not solid: cairo_plot_circle( cr, center=center, radius=radius + CC.texture_width, edgecolor=[0, 0, 0], width=CC.obstacle_border_width ) if texture is not None: if solid: width_outside = 0 width_inside = CC.texture_width else: width_inside = 0 width_outside = CC.texture_width cairo_plot_texture_circle( cr=cr, radius=radius, center=center, texture=texture, resolution=CC.texture_resolution, width_inside=width_inside, width_outside=width_outside, ) else: r1 = radius - CC.texture_width if r1 > 0: cairo_plot_circle( cr, center=center, radius=r1, edgecolor=None, facecolor=facecolor, width=CC.obstacle_border_width )