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
)