def _set_ref(self, new_ref_name): '''Changes the reference frame of the action step to new_ref_name. Args: new_ref_name ''' # Get the id of the new ref (an int). new_ref = world.get_ref_from_name(new_ref_name) if new_ref != ArmState.ROBOT_BASE: index = ActionStepMarker._ref_names.index(new_ref_name) new_ref_obj = ActionStepMarker._ref_object_list[index - 1] else: new_ref_obj = Landmark() if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). t = self.action_step.armTarget if self.arm_index == Side.RIGHT: t.rArm = world.convert_ref_frame(t.rArm, new_ref, new_ref_obj) else: t.lArm = world.convert_ref_frame(t.lArm, new_ref, new_ref_obj) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectory steps. t = self.action_step.armTrajectory arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm for i in range(len(arm)): arm_old = arm[i] arm_new = world.convert_ref_frame(arm_old, new_ref, new_ref_obj) arm[i] = arm_new # Fix up reference frames. if self.arm_index == Side.RIGHT: t.rRefFrameLandmark = new_ref_obj t.rRefFrame = new_ref else: t.lRefFrameLandmark = new_ref_obj t.lRefFrame = new_ref
def _find_dominant_ref(self, arm_traj, frame_list): '''Finds the most dominant reference frame in a continuous trajectory. Args: arm_traj (ArmState[]): List of arm states that form the arm trajectory. frame_list ([Landmark]): List of Landmark (as defined by Landmark.msg), the current reference frames. Returns: (int, Landmark): Tuple of the dominant reference frame's number (as one of the constants available in ArmState to be set as ArmState.refFrame) and Landmark (as in Landmark.msg). ''' # Cycle through all arm states and check their reference frames. # Whichever one is most frequent becomes the dominant one. robot_base = Landmark(name=BASE_LINK) ref_counts = Counter() for arm_state in arm_traj: # We only track objects that if arm_state.refFrame == ArmState.ROBOT_BASE: ref_counts[robot_base] += 1 elif arm_state.refFrameLandmark in frame_list: ref_counts[arm_state.refFrameLandmark] += 1 else: rospy.logwarn('Ignoring object with reference frame name ' + arm_state.refFrameLandmark.name + ' because the world does not have this object.') # Get most common obj. dominant_ref_obj = ref_counts.most_common(1)[0][0] # Find the frame number (int) and return with the object. return world.get_ref_from_name(dominant_ref_obj.name), dominant_ref_obj
def _update_viz_core(self): '''Updates visualization after a change.''' # Create a new IM control. menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True # Multiplex marker types added based on action step type. if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectories. # First, get all trajectory positions. point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) # Add a main maker for all points in the trajectory (sphere # list). # NOTE(jstn): This will not work anymore, trajectories are # unsupported for now. menu_control.markers.append( Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_TRAJ_STEP_SPHERES, header=Header(frame_id=frame_id), color=COLOR_TRAJ_STEP_SPHERES, points=point_list)) # Add a marker for the first step in the trajectory. menu_control.markers.append(ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_FIRST, self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE)) # Add a marker for the last step in the trajectory. last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_LAST, self._get_traj_pose(last_index), frame_id, TRAJ_ENDPOINT_SCALE)) else: # Neither "normal" pose nor trajectory; error... rospy.logerr( 'Non-handled action step type ' + str(self.action_step.type)) # Add an arrow to the relative object, if there is one. base_pose = world.get_absolute_pose(self.get_target()) # base_pose refers to the wrist link, which looks weird in # visualizations, so offset_pose is shifted forward to be in the # "middle" of the gripper offset_pose = ActionStepMarker._offset_pose(base_pose, 1) ref_frame = world.get_ref_from_name(self._get_ref_name()) if ref_frame == ArmState.OBJECT: menu_control.markers.append( Marker(type=Marker.ARROW, id=(ID_OFFSET_REF_ARROW + self.get_uid()), scale=SCALE_OBJ_REF_ARROW, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ_REF_ARROW, points=[offset_pose.position, self.get_target( ).refFrameLandmark.pose.position])) # Make and add the text for this step ('Step X'). text_pos = Point() text_pos.x = offset_pose.position.x text_pos.y = offset_pose.position.y text_pos.z = offset_pose.position.z + TEXT_Z_OFFSET menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=SCALE_STEP_TEXT, text='Step ' + str(self.step_number), color=COLOR_STEP_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = BASE_LINK int_marker.pose = offset_pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)