def plot_servonave(cr, servonav):
locations = servonav['locations']
# current_goal = servonav['current_goal']
for _, loc in enumerate(locations):
pose = SE2_from_SE3(SE3.from_yaml(loc['pose']))
with cairo_rototranslate(cr, pose):
# if current_goal == i:
# cairo_ref_frame(cr, l=0.5)
# else:
grey = [.6, .6, .6]
cairo_ref_frame(cr, l=0.5, x_color=grey, y_color=grey)
def extra_draw_world(cr):
for pose in poses:
with cairo_rototranslate(cr, SE2_from_SE3(pose)):
cairo_ref_frame(cr, l=0.5,
x_color=[0, 0, 0], y_color=[0, 0, 0])
def plot_servoing_poses(cr, servoing_poses):
# TODO
goal = SE3.from_yaml(servoing_poses['goal'])
with cairo_rototranslate(cr, goal):
cairo_ref_frame(cr, l=0.5)