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)