def initialize_viz(self, object_list):
'''Initialize visualization.
Args:
object_list ([Landmark]): List of Landmark (as defined by
Landmark.msg), the current reference frames.
'''
self.lock.acquire()
for i in range(len(self.seq.seq)):
step = self.seq.seq[i]
# NOTE(mbforbes): It's unclear to me this is the best way to
# support future step types in the system. Doesn't this just
# mark one more spot in the code that needs to be changed to
# implement another action type?
if (step.type == ActionStep.ARM_TARGET or
step.type == ActionStep.ARM_TRAJECTORY):
# Construct the markers.
r_marker = ActionStepMarker(
self._world,
i + 1, # step_number
Side.RIGHT, # arm_index
step, # action_step
self.marker_click_cb # marker_click_cb
)
l_marker = ActionStepMarker(
self._world,
i + 1, # step_number
Side.LEFT, # arm_index
step, # action_step
self.marker_click_cb # marker_click_cb
)
# Update and add.
r_marker.update_ref_frames(object_list)
l_marker.update_ref_frames(object_list)
self.r_markers.append(r_marker)
self.l_markers.append(l_marker)
# If we're not adding the first step, we should link the
# last one to it.
if i > 0:
self.r_links[i] = self._get_link(Side.RIGHT, i)
self.l_links[i] = self._get_link(Side.LEFT, i)
self._update_markers()
self.lock.release()
def add_action_step(self, step, object_list):
'''Function to add a new step to the action.
Args:
step (ActionStep): The new step to add.
object_list ([Landmark]): List of Landmark (as defined by
Landmark.msg), the current reference frames.
'''
self.lock.acquire()
self.seq.seq.append(self._copy_action_step(step))
# We currently support arm targets and arm trajectories.
# NOTE(mbforbes): It's unclear to me this is the best way to
# support future step types in the system. Doesn't this just
# mark one more spot in the code that needs to be changed to
# implement another action type?
if (step.type == ActionStep.ARM_TARGET
or step.type == ActionStep.ARM_TRAJECTORY):
# Create and append new action step markers.
# NOTE(mbforbes): One of many instances of code duplication
# b/c of right/left...
last_step = self.seq.seq[-1]
r_marker = ActionStepMarker(self._world, self.n_frames(),
Side.RIGHT, last_step,
self.marker_click_cb)
r_marker.update_ref_frames(object_list)
l_marker = ActionStepMarker(self._world, self.n_frames(),
Side.LEFT, last_step,
self.marker_click_cb)
l_marker.update_ref_frames(object_list)
self.r_markers.append(r_marker)
self.l_markers.append(l_marker)
# If we have any steps in this action, we link the previous
# one to this new one.
if self.n_frames() > 1:
self.r_links[self.n_frames() - 1] = self._get_link(
Side.RIGHT,
self.n_frames() - 1)
self.l_links[self.n_frames() - 1] = self._get_link(
Side.LEFT,
self.n_frames() - 1)
self.lock.release()
def _get_link(self, arm_index, to_index):
'''Returns a marker representing a link b/w two consecutive
action steps (both must already exist).
Args:
arm_index (int): Side.RIGHT or Side.LEFT
to_index (int): The index of the 'end' marker (the latter of
the two).
Returns:
Marker
'''
markers = self.r_markers if arm_index == Side.RIGHT else self.l_markers
start = markers[to_index - 1].get_absolute_position(is_start=True)
end = markers[to_index].get_absolute_position(is_start=False)
return Marker(type=Marker.ARROW,
id=ActionStepMarker.calc_uid(arm_index, to_index),
lifetime=LINK_MARKER_LIFETIME,
scale=LINK_SCALE,
header=Header(frame_id=BASE_LINK),
color=LINK_COLOR,
points=[start, end])