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])