def plot_graph_school(self):
bloat = True
per_list = [per for per in self.z_values.keys()]
n_sub = len(per_list)
for i in range(n_sub):
if i == 0:
continue
fig = plt.figure(figsize=(6, 4), dpi=200)
# fig_1, ax_arr = plt.subplots(1, 1, figsize=(9, 5), dpi=150)
mg.plot_ss2(bloat, True, 'dimgray')
mg.plot_graph(bloat, True, True)
V = ff.get_info(self.graph_vertices, 'V')
danger = True
if danger:
colors = self.get_vertices_color(per_list[i])
for v in V:
vidx = V.index(v)
my_color = colors[vidx]
self.plot_points([v], my_color, 'o', 6)
xy = [0.125, 0.15]
rpf.my_hazard_labels(fig, xy, 14)
plt.xlim(right=72, left=-7) # adjust the right leaving left unchanged
plt.ylim(bottom=-0.5, top=59)
plt.axis('off')
my_str = 'School Scenario' #'Danger Graph, ' + str(per_list[i]) + '\% images'
# plt.title(my_str, fontsize=20, weight='heavy')
# save fig
fig_path = MyDanger.get_folder_path('figs')
fig_name = fig_path + '/ss2' + '.pdf'
fig.savefig(fig_name, bbox_inches='tight')
fig_name = 'danger_graph_' + str(per_list[i])
mg.save_plot(fig_name, 'figs', '.pdf')
def plot_pose(f_name='School_RPose', finish=False, color='b-'):
"""Plot robot pose as extracted from csv file"""
if isinstance(f_name, str):
# get pose info
RPose = ff.get_info(f_name, 'R')
else:
RPose = f_name
print(len(RPose))
R_x = [el[0] for el in RPose[1000:]]
R_y = [el[1] for el in RPose[1000:]]
plt.plot(R_x, R_y, color)
if finish:
finish_plot("pose", ['Robot'])
def align_pose_school():
# frame details - robot entering gym
# visual: 1032, (4.85, -15.29)
pose_raw = ff.get_info('School_RPose', 'R')
frame_number = 1040
xy_frame = pose_raw[frame_number]
# SCHOOL
bloat = True
school = plot_ss2(bloat)
# floorplan detail - gym door
xy_door = (school[1].c1[0], school[1].c1[1] + a_x * (2 + 0.9) + 0.5)
# and robot POSE
plot_pose(pose_raw, False, 'c')
# plot intersection points
pts = [xy_frame, xy_door]
pf.plot_points_between_list(pts, [(0, 1)], 'r', 'o')
# translate
delta = (round(xy_door[0] - xy_frame[0],
2), round(xy_door[1] - xy_frame[1], 2))
print(delta)
pose2 = ff.trans_pose(delta)
# save pose translated
ff.make_txt('School_RPose_Translated', pose2,
['SS-2', 'Robot Pose Translated'], ["ID", 'x', 'y'])
origin = [(0, 0), (-50, 0), (60, 0), (0, -20), (0, 60)]
or_conn = [(0, 1), (0, 2), (0, 3), (0, 4)]
plot_pose(pose2, False, 'blue')
pf.plot_points_between_list(origin, or_conn, 'gray')
# plot_graph()
lgd = ['Origin', 'School', 'Robot', 'Matching Point', 'Robot (raw)']
finish_plot('mis', lgd)
def plot_graph(bloat=False, number=False, edges=True, finish=False):
"""Plot graph (not numbered)"""
# get graph info
if bloat:
f_name = ['School_Image_VGraph', 'School_Image_EGraph']
else:
f_name = ['School_Gazebo_VGraph', 'School_Gazebo_EGraph']
# retrieve vertices and edges
V = ff.get_info(f_name[0], 'V')
E = ff.get_info(f_name[1], 'E')
my_color = 'k'
if edges:
# organize edges as list with python index (0...n-1)
E_plot = []
for el in E:
v1 = el[0] - 1
v2 = el[1] - 1
E_plot.append((v1, v2))
pf.plot_points_between_list(V, E_plot, 'dimgray', 'o', 2)
else:
pf.plot_points(V, my_color, 'o', 2)
if number:
font_v = FontProperties()
alignment = {
'horizontalalignment': 'center',
'verticalalignment': 'center'
}
families = ['serif', 'sans-serif', 'cursive', 'fantasy', 'monospace']
font_v.set_size(8)
font_v.set_weight('bold')
font_v.set_family('fantasy')
# plot index + 1 at v coordinates +- offset
for coord in V:
i = V.index(coord) + 1
if i < 10:
dx, dy = 0.5, 0.5
elif i < 40:
dx, dy = 0.7, 0.62
else:
dx, dy = 0.75, 0.62
dx, dy = 0, 0
x = coord[0] - dx # - 0.5
y = coord[1] - dy #+ 0.75
plt.text(x,
y,
str(i),
fontproperties=font_v,
color='indigo',
**alignment)
if finish:
finish_plot('graph')
def plot_graph_school(self):
bloat = True
per_list = [per for per in self.z_values.keys()]
n_sub = len(per_list)
for i in range(n_sub):
# if i == 0:
# continue
fig = plt.figure(figsize=(6, 4), dpi=200)
# fig_1, ax_arr = plt.subplots(1, 1, figsize=(9, 5), dpi=150)
mg.plot_ss2(bloat, True, 'darkgray')
mg.plot_graph(bloat, True, True)
V = ff.get_info(self.graph_vertices, 'V')
danger = True
if danger:
colors = self.get_vertices_color(per_list[i])
for v in V:
vidx = V.index(v)
my_color = colors[vidx]
self.plot_points([v], my_color, 'o', 13) # 6
xy = [0.15, 0.15] # [0.125, 0.15]
if i < 1:
rpf.my_hazard_labels(fig, xy, 12)
ha = 0.88
wa = 0.73
xy2 = [[0.725, 0.515], [0.135, 0.799], [0.27, 0.81], [0.45, ha],
[0.57, ha], [0.69, ha], [0.8, ha], [wa, 0.585], [wa, 0.635],
[wa, 0.685], [0.90, 0.6]]
rooms = [
'GYM', 'CAFE', 'HALL 2', 'CLASS D', 'CLASS C', 'CLASS B',
'CLASS A', 'STGE', 'WC1', 'WC2', 'HALL 1'
]
rpf.my_rooms_label(fig, xy2, rooms, 9)
plt.xlim(right=72,
left=-7) # adjust the right leaving left unchanged
plt.ylim(bottom=-0.5, top=59)
plt.axis('off')
my_str = 'School Scenario'
# save fig
fig_path = MyDanger.get_folder_path('figs')
fig_name = fig_path + '/ss2' + '.pdf'
fig.savefig(fig_name, bbox_inches='tight')
fig_name = 'danger_graph_' + str(per_list[i])
mg.save_plot(fig_name, 'figs', '.pdf')