def plot_ranges_compact(cr, directions, readings, valid,
r=CC.laser_compact_r,
r_width=CC.laser_compact_r_width):
if len(directions) >= 2:
delta_theta = np.abs(directions[2] - directions[1])
else:
delta_theta = CC.delta_single_ray
N = directions.size
vmax = np.nanmax(readings)
readings = readings / vmax
theta1 = directions - delta_theta / 1.9
theta2 = directions + delta_theta / 1.9
r0 = r * np.ones(readings.size)
r1 = r + readings * r_width
for i in range(N):
if not valid[i]:
continue
cairo_set_color(cr, CC.laser_compact_bg)
cairo_fill_slice(cr, theta1[i], theta2[i], r, r + r_width)
for i in range(N):
if not valid[i]:
continue
cairo_set_color(cr, CC.laser_compact_fg)
cairo_fill_slice(cr, theta1[i], theta2[i], r0[i], r1[i])
def cairo_robot_skin_brai(cr, w=1.3, h=0.8, sensors=False):
with cairo_save(cr):
x0 = -.3
wheel_h = 0.15
wheel_w = 0.4
M = h / 2 + .8 * wheel_h
def body():
roundedrec(cr, x0, -h / 2, w, h, r=min(w, h) / 8.0)
# cr.rectangle(x0, -h / 2, w, h)
cr.move_to(0, M)
cr.line_to(0, -M)
cairo_set_color(cr, BLACK)
cr.set_line_width(0.05)
cr.stroke()
cairo_plot_with_style(cr, body, **CairoConstants.robot_body_style)
for y in [-M, +M]:
with cairo_transform(cr, t=[0, y]):
wheel(cr, w=wheel_w, h=wheel_h)
if sensors:
for y in [.3 * h, -.3 * h]:
with cairo_transform(cr, t=[0, y]):
cr.set_line_width(CairoConstants.robot_border_width)
cairo_set_color(cr, CairoConstants.robot_border_color)
cr.move_to(0, 0)
cr.line_to(1.1, 0)
cr.stroke()
r = .1
with cairo_transform(cr, t=[1.1 + r, 0]):
cr.arc(0, 0, r, +np.pi / 2, -np.pi / 2)
cr.stroke()
def cairo_arrow(cr, length, caplen=0.05, capwidth=0.05,
border_width=0.05, border_color=[0, 0, 0]):
cr.set_line_width(border_width)
cairo_set_color(cr, border_color)
cr.move_to(0, 0)
cr.line_to(length, 0)
cr.stroke()
cr.move_to(length - caplen, capwidth / 2.0)
cr.line_to(length, 0)
cr.line_to(length - caplen, -capwidth / 2.0)
cr.fill()
def cairo_plot_polyline_primitive(cr, points, texture=None):
with cairo_save(cr):
cr.set_line_width(CC.obstacle_border_width)
cairo_set_color(cr, CC.obstacle_border_color)
cairo_plot_polyline(cr, points[0, :], points[1, :])
if texture is not None:
cairo_plot_textured_polyline(cr,
points=points,
texture=texture,
resolution=CC.texture_resolution,
width_inside=0,
width_outside=CC.texture_resolution)
def cairo_plot_polyline_primitive(cr, points, texture=None):
with cairo_save(cr):
cr.set_line_width(CC.obstacle_border_width)
cairo_set_color(cr, CC.obstacle_border_color)
cairo_plot_polyline(cr, points[0, :], points[1, :])
if texture is not None:
cairo_plot_textured_polyline(
cr,
points=points,
texture=texture,
resolution=CC.texture_resolution,
width_inside=0,
width_outside=CC.texture_resolution,
)
def plot_photoreceptors_compact(cr, directions, valid, luminance,
r=CC.photoreceptors_compact_r,
r_width=CC.photoreceptors_compact_r_width):
if len(directions) >= 2:
delta_theta = np.abs(directions[2] - directions[1])
else:
delta_theta = CC.delta_single_ray
N = directions.size
for i in range(N):
if not valid[i]:
continue
theta1 = directions[i] - delta_theta / 1.9
theta2 = directions[i] + delta_theta / 1.9
r0 = r
r1 = r + r_width
cairo_set_color(cr, luminance2color(luminance[i]))
cairo_fill_slice(cr, theta1, theta2, r0, r1)
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)
