class RobocupInteractiveMarker(object):
def __init__(self, server):
self.server = server
self.pose = Pose()
self.publish = True
self.int_marker = None
self.make_marker()
self.menu_handler = MenuHandler()
item = self.menu_handler.insert("publish", callback=self.menu_callback)
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
self.menu_handler.apply(self.server, self.marker_name)
def feedback(self, feedback):
self.pose = feedback.pose
self.server.applyChanges()
def menu_callback(self, feedback):
item = feedback.menu_entry_id
if self.menu_handler.getCheckState(item) == MenuHandler.CHECKED:
self.menu_handler.setCheckState(item, MenuHandler.UNCHECKED)
self.publish = False
else:
self.publish = True
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def make_marker(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "map"
self.int_marker.pose = self.pose
self.int_marker.scale = 1
self.int_marker.name = self.marker_name
control = InteractiveMarkerControl()
control.orientation.w = math.sqrt(2) / 2
control.orientation.x = 0
control.orientation.y = math.sqrt(2) / 2
control.orientation.z = 0
control.interaction_mode = self.interaction_mode
self.int_marker.controls.append(copy.deepcopy(control))
# make a box which also moves in the plane
markers = self.make_individual_markers(self.int_marker)
for marker in markers:
control.markers.append(marker)
control.always_visible = True
self.int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(self.int_marker, self.feedback)
class ActionStepMarker:
'''Marker for visualizing the steps of an action.'''
_im_server = None
_offset = DEFAULT_OFFSET
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step, marker_click_cb):
'''
Args:
step_number (int): The 1-based index of the step.
arm_index (int): Side.RIGHT or Side.LEFT
action_step (ActionStep): The action step this marker marks.
marker_click_cb (function(int,bool)): The function to call
when a marker is clicked. Pass the uid of the marker
(as calculated by get_uid(...) as well as whether it's
selected.
'''
if ActionStepMarker._im_server is None:
im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.arm_name = ARM_NAMES[arm_index]
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
self._prev_is_reachable = None
ActionStepMarker._marker_click_cb = marker_click_cb
# ##################################################################
# Static methods: Public (API)
# ##################################################################
@staticmethod
def calc_uid(arm_index, step_number):
'''Returns a unique id of the marker of the arm_index arm with
step_number step.
Args:
arm_index (int): Side.RIGHT or Side.LEFT
step_number (int): The number of the step.
Returns:
int: A number that is unique given the step number and arm
index.
'''
return len(ARM_NAMES) * step_number + arm_index
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _make_sphere_marker(uid, pose, frame_id, radius):
'''Creates and returns a sphere marker.
Args:
uid (int): The id to use for the marker.
pose (Pose): The pose of the marker.
frame_id (str): Frame the marker is associated with. (See
std_msgs/Header.msg for more info.)
radius (float): Amount to scale the marker by. Scales in all
directions (x, y, and z).
Returns:
Marker
'''
return Marker(
type=Marker.SPHERE,
id=uid,
lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose,
header=Header(frame_id=frame_id),
color=COLOR_TRAJ_ENDPOINT_SPHERES
)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization.
Args:
pose (Pose): The pose to offset.
constant (int, optional): How much to scale the set offset
by (scales ActionStepMarker._offset). Defaults to 1.
Returns:
Pose: The offset pose.
'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return World.get_pose_from_transform(hand_transform)
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def get_uid(self):
'''Returns a unique id for this marker.
Returns:
int: A number that is unique given the step number and arm
index.
'''
return ActionStepMarker.calc_uid(self.arm_index, self.step_number)
def decrease_id(self):
'''Reduces the step index of the marker.'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes.
Args:
ref_frame_list ([Object]): List of Object.msg objects, the
reference frames of the system.
'''
# There is a new list of objects. If the current frame is
# relative (already assigned to an object) we need to figure out
# the correspondences.
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if arm_pose.refFrame == ArmState.OBJECT:
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(
prev_ref_obj, ref_frame_list)
if new_ref_obj is not None:
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
else:
self.has_object = False
# Re-populate cached list of reference names.
ActionStepMarker._ref_names = [BASE_LINK]
for obj in ActionStepMarker._ref_object_list:
ActionStepMarker._ref_names.append(obj.name)
self._update_menu()
def destroy(self):
'''Removes marker from the world.'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def set_new_pose(self, new_pose):
'''Changes the pose of the action step to new_pose.
Args:
new_pose (Pose)
'''
if self.action_step.type == ActionStep.ARM_TARGET:
t = self.action_step.armTarget
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
arm.ee_pose = ActionStepMarker._offset_pose(new_pose, -1)
self.update_viz()
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
rospy.logwarn(
'Modification of whole trajectory segments is not ' +
'implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step.
Args:
is_start (bool, optional). For trajectories only. Whether to
get the final position in the trajectory. Defaults to
True.
Returns:
Point
'''
return self.get_absolute_pose(is_start).position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step.
Args:
is_start (bool, optional). For trajectories only. Whether to
get the final pose in the trajectory. Defaults to True.
Returns:
Pose
'''
if self.action_step.type == ActionStep.ARM_TARGET:
# "Normal" saved pose.
t = self.action_step.armTarget
arm_pose = t.rArm if self.arm_index == Side.RIGHT else t.lArm
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Trajectory.
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
# TODO(mbforbes): Make sure this isn't a bug in the original
# implementation. Wouldn't is_start imply you want the first
# one?
index = len(arm) - 1 if is_start else 0
arm_pose = arm[index]
# TODO(mbforbes): Figure out if there are cases that require
# this, or remove.
# if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
# else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step.
Returns:
Pose
'''
target = self.get_target()
if target is not None:
return ActionStepMarker._offset_pose(target.ee_pose)
def set_target(self, target):
'''Sets the new ArmState for this action step.
Args:
target (ArmState): Replacement for this target.
'''
if self.action_step.type == ActionStep.ARM_TARGET:
at = self.action_step.armTarget
if self.arm_index == Side.RIGHT:
at.rArm = target
else:
at.lArm = target
# TODO(mbforbes): Why is self.has_object set to True here?
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the ArmState for this action step.
Args:
traj_index (int, optional): Which step in the trajectory to
return the ArmState for. Only applicable for
trajectories (not "normal" saved poses). Defaults to
None, in which case the middle point is used.
Returns:
ArmState
'''
if self.action_step.type == ActionStep.ARM_TARGET:
t = self.action_step.armTarget
return t.rArm if self.arm_index == Side.RIGHT else t.lArm
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
# If traj_index not passed, use the middle one.
if traj_index is None:
traj_index = int(len(arm) / 2)
return arm[traj_index]
def update_viz(self):
'''Updates visualization fully.'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def pose_reached(self):
'''Update when a requested pose is reached.'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested.
Args:
feedback (InteractiveMarkerFeedback (?))
'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(
feedback.menu_entry_id, MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo(
'Switching reference frame to ' + new_ref + ' for action step ' +
self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller.
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Changing visibility of the pose controls.')
self.is_control_visible = not self.is_control_visible
ActionStepMarker._marker_click_cb(
self.get_uid(), self.is_control_visible)
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
# TODO(mbforbes): Figure out types of objects sent to these
# callbacks.
def delete_step_cb(self, __):
'''Callback for when delete is requested.
Args:
__ (???): Unused
'''
self.is_deleted = True
def move_to_cb(self, __):
'''Callback for when moving to a pose is requested.
Args:
__ (???): Unused
'''
self.is_requested = True
def move_pose_to_cb(self, __):
'''Callback for when a pose change to current is requested.
Args:
__ (???): Unused
'''
self.is_edited = True
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _is_reachable(self):
'''Checks and returns whether there is an IK solution for this
action step.
Returns:
bool: Whether this action step is reachable.
'''
dummy, is_reachable = Arms.solve_ik_for_arm(
self.arm_index, self.get_target())
# A bit more complicated logging to avoid spamming the logs
# while still giving useful info. It now logs when reachability
# is first calculated, or changes.
report = False
# See if we've set reachability for the first time.
if self._prev_is_reachable is None:
report = True
reachable_str = (
'is reachable' if is_reachable else 'is not reachable')
# See if we've got a different reachability than we had before.
elif self._prev_is_reachable != is_reachable:
report = True
reachable_str = (
'is now reachable' if is_reachable else
'is no longer reachable')
# Log if it's changed.
if report:
rospy.loginfo(
'Pose (' + str(self.step_number) + ', ' + self.arm_name
+ ') ' + reachable_str)
# Cache and return.
self._prev_is_reachable = is_reachable
return is_reachable
def _get_name(self):
'''Generates the unique name for the marker.
Returns:
str: A human-readable unique name for the marker.
'''
return 'step' + str(self.step_number) + 'arm' + str(self.arm_index)
def _update_menu(self):
'''Recreates the menu when something has changed.'''
self._menu_handler = MenuHandler()
# Insert sub entries.
self._sub_entries = []
frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
refs = ActionStepMarker._ref_names
for ref in refs:
subent = self._menu_handler.insert(
ref, parent=frame_entry, callback=self.change_ref_cb)
self._sub_entries += [subent]
# Inset main menu entries.
self._menu_handler.insert(
MENU_OPTIONS['move_here'], callback=self.move_to_cb)
self._menu_handler.insert(
MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb)
self._menu_handler.insert(
MENU_OPTIONS['del'], callback=self.delete_step_cb)
# Make all unchecked to start.
for subent in self._sub_entries:
self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED)
# Check if necessary.
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
# Update.
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name or None if the
object is not found.
Returns:
int (?)|None
'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id.
Returns:
str
'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference frame object of the
action step.
Returns:
str|None: Under all normal circumstances, returns the str
reference frame name. Returns None in error.
'''
ref_name = None
if self.action_step.type == ActionStep.ARM_TARGET:
# "Normal" step (saved pose).
t = self.action_step.armTarget
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
ref_frame = arm.refFrame
ref_name = arm.refFrameObject.name
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# "Trajectory" step.
t = self.action_step.armTrajectory
if self.arm_index == Side.RIGHT:
ref_frame = t.rRefFrame
ref_name = t.rRefFrameObject.name
else:
ref_frame = t.lRefFrame
ref_name = t.lRefFrameObject.name
else:
rospy.logerr(
'Unhandled marker type: ' + str(self.action_step.type))
# Update ref frame name if it's absolute.
if ref_frame == ArmState.ROBOT_BASE:
ref_name = BASE_LINK
return ref_name
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 = Object()
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.rRefFrameObject = new_ref_obj
t.rRefFrame = new_ref
else:
t.lRefFrameObject = new_ref_obj
t.lRefFrame = new_ref
def _is_hand_open(self):
'''Returns whether the gripper is open for this action step.
Returns:
bool
'''
ga = self.action_step.gripperAction
gstate = ga.rGripper if self.arm_index == Side.RIGHT else ga.lGripper
return gstate.state == GripperState.OPEN
def _get_traj_pose(self, index):
'''Returns this trajectory's pose at index. Only applicable for
trajectories.
Args:
index (int): Which step in the trajectory to return the
pose from.
Returns:
Pose
'''
if self.action_step.type == ActionStep.ARM_TRAJECTORY:
at = self.action_step.armTrajectory
arm_states = at.rArm if self.arm_index == Side.RIGHT else at.lArm
return arm_states[index].ee_pose
else:
rospy.logerr(
'Cannot request trajectory pose on non-trajectory action ' +
'step.')
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
frame_id = self._get_ref_name()
pose = self.get_pose()
# 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).
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.
ref_frame = World.get_ref_from_name(frame_id)
if ref_frame == ArmState.OBJECT:
menu_control.markers.append(
Marker(
type=Marker.ARROW,
id=(ID_OFFSET_REF_ARROW + self.get_uid()),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_OBJ_REF_ARROW,
header=Header(frame_id=frame_id),
color=COLOR_OBJ_REF_ARROW,
points=[pose.position, Point(0, 0, 0)]
)
)
# Make and add the text for this step ('Step X').
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = 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=frame_id),
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 = frame_id
int_marker.pose = 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)
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker.
Args:
int_marker (InteractiveMarker)
is_fixed (bool): Looks like whether position is fixed (?).
Currently only passed as True.
'''
# Each entry in options is (name, orientation, is_move)
options = [
('rotate_x', Quaternion(1, 0, 0, 1), False),
('move_x', Quaternion(1, 0, 0, 1), True),
('rotate_z', Quaternion(0, 1, 0, 1), False),
('move_z', Quaternion(0, 1, 0, 1), True),
('rotate_y', Quaternion(0, 0, 1, 1), False),
('move_y', Quaternion(0, 0, 1, 1), True),
]
for opt in options:
name, orient, is_move = opt
control = self._make_6dof_control(name, orient, is_move, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for hte control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self.is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates and returns a mesh marker.
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = self._get_mesh_marker_color()
return mesh
def _get_mesh_marker_color(self):
'''Gets the color for the mesh marker (thing that looks like a
gripper) for this step.
A simple implementation of this will return one color for
reachable poses, another for unreachable ones. Future
implementations may provide further visual cues.
Returns:
ColorRGBA: The color for the gripper mesh for this step.
'''
if self._is_reachable():
return COLOR_MESH_REACHABLE
else:
return COLOR_MESH_UNREACHABLE
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker, adds it to control, returns control.
Args:
control (InteractiveMarkerControl): IM Control we're using.
is_hand_open (bool, optional): Whether the gripper is open.
Defaults to False (closed).
Returns:
InteractiveMarkerControl: The passed control.
'''
# Set angle of meshes based on gripper open vs closed.
angle = ANGLE_GRIPPER_OPEN if is_hand_open else ANGLE_GRIPPER_CLOSED
# Make transforms in preparation for meshes 1, 2, and 3.
# NOTE(mbforbes): There are some magic numbers in here that are
# used a couple times. Seems like a good candidate for
# refactoring to constants, but I think they're more clear if
# left in here as (a) they likely won't be changed, and (b) it's
# easier to understand the computations with them here.
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 1 (palm).
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
# Create mesh 2 (finger).
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = World.get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = World.get_pose_from_transform(t_distal)
# Make transforms in preparation for meshes 4 and 5.
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle)
)
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 4 (other finger).
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = World.get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = World.get_pose_from_transform(t_distal)
# Append all meshes we made.
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
# Return back the control.
# TODO(mbforbes): Why do we do this?
return control
class ActionStepMarker:
''' Marker for visualizing the steps of an action'''
_im_server = None
_offset = 0.09
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step, marker_click_cb):
if ActionStepMarker._im_server == None:
im_server = InteractiveMarkerServer('programmed_actions')
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
ActionStepMarker._marker_click_cb = marker_click_cb
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Arms.solve_ik_for_arm(self.arm_index,
self.get_target())
rospy.loginfo('Reachability of pose in ActionStepMarker : ' +
str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number) + 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(prev_ref_obj,
ref_frame_list)
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
ActionStepMarker._ref_names = ['base_link']
for i in range(len(ActionStepMarker._ref_object_list)):
ActionStepMarker._ref_names.append(
ActionStepMarker._ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(ActionStepMarker._ref_names)
for i in range(len(ActionStepMarker._ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
ActionStepMarker._ref_names[i],
parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(ActionStepMarker._ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id == None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameObject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameObject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
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 = Object()
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
self.update_viz()
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ActionStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return World.get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index == None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index == None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(self.get_uid() + 2000,
self._get_traj_pose(0),
frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type ' +
str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(
Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE,
id=uid,
lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose,
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ActionStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to ' + new_ref +
' for action step ' + self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1),
True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class ActionStepMarker:
''' Marker for visualizing the steps of an action'''
_im_server = None
_offset = 0.09
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step,
marker_click_cb):
if ActionStepMarker._im_server == None:
im_server = InteractiveMarkerServer('programmed_actions')
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
ActionStepMarker._marker_click_cb = marker_click_cb
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Arms.solve_ik_for_arm(self.arm_index,
self.get_target())
rospy.loginfo('Reachability of pose in ActionStepMarker : ' +
str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number)
+ 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(prev_ref_obj,
ref_frame_list)
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
ActionStepMarker._ref_names = ['base_link']
for i in range(len(ActionStepMarker._ref_object_list)):
ActionStepMarker._ref_names.append(
ActionStepMarker._ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(ActionStepMarker._ref_names)
for i in range(len(ActionStepMarker._ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
ActionStepMarker._ref_names[i], parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(ActionStepMarker._ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id == None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id,
MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameObject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameObject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
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 = Object()
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
self.update_viz()
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ActionStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return World.get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index == None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index == None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker,
self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose, header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ActionStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()),
MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to '
+ new_ref + ' for action step ' + self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x',
Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x',
Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z',
Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z',
Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y',
Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y',
Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class SimpleInteractiveMarker(object):
"""Creates a group of markers whose poses can be edited."""
def __init__(self, server, name, X_WM, markers):
self._server = server
self.name = name
self._X_WM = X_WM
self._menu = MenuHandler()
self._edit_id = self._menu.insert("Edit", callback=self._edit_callback)
self._delete_id = self._menu.insert("Delete", callback=self._delete_callback)
self._menu.insert(self.name, callback=noop) # For annotation.
self._t_click_prev = -float('inf')
# Make template marker.
template = InteractiveMarker()
template.name = self.name
template.description = name
template.header.frame_id = "world"
viz = InteractiveMarkerControl()
viz.interaction_mode = InteractiveMarkerControl.BUTTON
viz.always_visible = True
viz.markers.extend(markers)
template.controls.append(viz)
self._template = template
# Initialize.
self._edit = False
self._update()
def get_pose(self):
return self._X_WM
def set_pose(self, X_WM):
self._X_WM = X_WM
self._server.setPose(self.name, self._X_WM)
self._server.applyChanges()
def delete(self):
self._X_WM = None
self._server.erase(self.name)
self._server.applyChanges()
def _update(self):
# Construct interactive marker.
marker = copy.deepcopy(self._template)
marker.pose = self._X_WM
if self._edit:
marker.controls += make_6dof_controls()
state = MenuHandler.CHECKED
else:
state = MenuHandler.UNCHECKED
self._menu.setCheckState(self._edit_id, state)
self._server.insert(marker, self._marker_callback)
self._menu.apply(self._server, marker.name)
self._server.applyChanges()
def _marker_callback(self, msg):
if msg.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self._X_WM = msg.pose
if msg.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Require double-click.
t_click = time.time()
dt_click = t_click - self._t_click_prev
self._t_click_prev = t_click
if dt_click < DT_DOUBLE_CLICK:
self._edit = not self._edit
self._update()
def _edit_callback(self, _):
# Toggle.
self._edit = not self._edit
self._update()
def _delete_callback(self, _):
self.delete()
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
def add_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
class ArmStepMarker:
''' Marker for visualizing the steps of an action'''
_offset = 0.09
_marker_click_cb = None
_reference_change_cb = None
def __init__(self, step_number, arm_index, action_step,
marker_click_cb, reference_change_cb, parent_sequence):
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self.is_dimmed = False
self.is_fake = False
self._sub_entries = None
self._menu_handler = None
ArmStepMarker._marker_click_cb = marker_click_cb
ArmStepMarker._reference_change_cb = reference_change_cb
self.parent_step_sequence = parent_sequence
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Robot.solve_ik_for_arm(self.arm_index,
self.get_target())
# rospy.loginfo('Reachability of pose in ArmStepMarker : ' +
# str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number)
+ 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list, new_ref_obj):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
self.parent_step_sequence.ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
self.parent_step_sequence.ref_names = ['base_link']
for i in range(len(self.parent_step_sequence.ref_object_list)):
self.parent_step_sequence.ref_names.append(
self.parent_step_sequence.ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
self.parent_step_sequence.im_server.erase(self._get_name())
self.parent_step_sequence.im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(self.parent_step_sequence.ref_names)
for i in range(len(self.parent_step_sequence.ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
self.parent_step_sequence.ref_names[i], parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(self.parent_step_sequence.ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id,
MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(self.parent_step_sequence.im_server, self._get_name())
self.parent_step_sequence.im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in self.parent_step_sequence.ref_names:
index = self.parent_step_sequence.ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return self.parent_step_sequence.ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameOject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameOject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
new_ref = World.get_world().get_ref_from_name(new_ref_name)
if (new_ref != ArmState.ROBOT_BASE):
index = self.parent_step_sequence.ref_names.index(new_ref_name)
new_ref_obj = self.parent_step_sequence.ref_object_list[index - 1]
else:
new_ref_obj = Object()
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.get_world().convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.get_world().convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.get_world().convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.get_world().convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
ArmStepMarker._reference_change_cb(self.get_uid(), new_ref, new_ref_obj)
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
pose = ArmStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ArmStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
rospy.loginfo('Set new pose for action step.')
self.update_viz()
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ArmStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_world().get_absolute_pose(arm_pose)
return ArmStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ArmStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_world().get_matrix_from_pose(pose)
offset_array = [constant * ArmStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return World.get_world().get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index is None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index is None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ArmStepType.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_world().get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
# The following is needed to properly display the arrtow in browser, due to the fact that ros3djs
# displays all nested markers in the reference frame of the interactive marker.
# Thus, we need to calculate the position of the object in the reference frame of the interactive marker.
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
inv_quat_matrix = quaternion_matrix(quaternion_inverse(quat))
pose_vec = numpy.array((pose.position.x, pose.position.y, pose.position.z, 0))
new_pose = numpy.dot(inv_quat_matrix, pose_vec)
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.01, 0.01, 0.0001),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.8, 0.0, 0.6),
points=[Point(0, 0, 0), Point(-new_pose[0], -new_pose[1], -new_pose[2])]))
# Calculate text position so that they "orbit" around the marker;
# this is done so that poses in identical or similar positions
# have non-overlapping text. Note that to do this without moving
# the text around as the camera is moved, we assume that the viewer
# is always looking directly at the robot, so we assume the x dimension
# is constant and "orbin" in the y-z plane.
n_orbitals = 8 # this should be a constant
offset = 0.15 # this should be a constant
orbital = (self.step_number - 1) % n_orbitals # - 1 to make 0-based
angle_rad = (float(orbital) / n_orbitals) * (-2 * numpy.pi) + \
(numpy.pi / 2.0) # start above, at pi/2 (90 degrees)
text_pos = Point()
text_pos.x = 0
text_pos.y = numpy.cos(angle_rad) * offset
text_pos.z = numpy.sin(angle_rad) * offset
r,g,b = self.get_marker_color()
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0.06, 0.06, 0.06),
lifetime=rospy.Duration(1.5),
text='Step ' + str(self.step_number),
color=ColorRGBA(r, g, b, 1.0),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
self.parent_step_sequence.im_server.insert(int_marker,
self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose, header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ArmStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()),
MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to '
+ new_ref + ' for action step ' + self._get_name())
self._menu_handler.reApply(self.parent_step_sequence.im_server)
self.parent_step_sequence.im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(self.parent_step_sequence.im_server)
self.parent_step_sequence.im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x',
Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x',
Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z',
Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z',
Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y',
Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y',
Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def get_marker_color(self):
'''Makes marker colors in a gradient according to the progression set in
_get_rgb_from_abs_pos() (below).
returns (r,g,b) tuple, each from 0.0 to 1.0
'''
total = self.parent_step_sequence.total_n_markers
# These are 1-based indexing; turn them into 0-based indexing.
idx = self.step_number - 1
# First calculate "absolute" color position, plotting on a 0.0 - 1.0
# scale. This only applies if there's more than one step; otherwise we
# set to 0.0 (though it could be anything, as it just gets multiplied
# by 0, as the idx must be 0 if there's only one step total).
abs_step_size = 1.0 / float(total - 1) if total > 1 else 0.0
abs_pos = abs_step_size * idx
# Then, use our helper method to turn this into an RGB value.
return self._get_rgb_from_abs_pos(abs_pos)
def _get_rgb_from_abs_pos(self, abs_pos):
'''Turns abs_pos, a float from 0.0 to 1.0 inclusive, into an rgb value
of the range currently programmed. The progression is as follows (in
order to avoid green hues, which could be confused with the objects),
by gradient "bucket" step:
- 0 yellow (start) -> red
- 1 red -> purple
- 2 purple -> blue
- 3 blue -> cyan (end)
Returns (r,g,b) tuple of floats, each from 0.0 to 1.0 inclusive.'''
# Bucket settings (make constant)
bucket = self.arm_index
bucket_pos = abs_pos
# Now translate to colors; todo later implement with better data
# structure.
r = 0.0
g = 0.0
b = 0.0
if bucket == 0:
# yellow -> red
r = 1.0
g = 1.0 - bucket_pos
elif bucket == 1:
# cyan -> blue
g = 1.0 - bucket_pos
b = 1.0
else:
# Set white as error color
rospy.logwarn("Bad color gradient; bucket " + str(bucket) +
" and bucket position " + str(bucket_pos))
r,g,b = 1.0, 1.0, 1.0
return (r,g,b)
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
alpha = 0.6
if self.is_dimmed:
alpha = 0.1
if self.is_fake:
alpha = 0.4
mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha)
return mesh
if self._is_reachable():
# Original: some kinda orange
# r,g,b = 1.0, 0.5, 0.0
# New: rainbow! See method comment for details.
r,g,b = self.get_marker_color()
mesh.color = ColorRGBA(r, g, b, alpha)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ArmStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_world().get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_world().get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_world().get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_world().get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class IntCollisionEnv(CollisionEnvServices):
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_server.applyChanges()
def ignored_cb(self, feedback):
pass
def d_menu_clear_cb(self, f):
pass
def global_menu_det_clear_all_cb(self, f):
for name in self.clear_all_det():
self.im_server.erase(name)
self.im_server.applyChanges()
def global_menu_pause_cb(self, f):
handle = f.menu_entry_id
state = self.global_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.global_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
self.paused = False
else:
self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.paused = True
self.global_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def global_menu_save_all_cb(self, f):
self.save_primitives()
def global_menu_add_prim_cb(self, feedback):
p = CollisionPrimitive()
p.name = self._generate_name()
p.bbox.type = SolidPrimitive.BOX
p.bbox.dimensions = [0.1, 0.1, 0.1]
p.pose.header.frame_id = self.world_frame
p.pose.pose.orientation.w = 1
p.pose.pose.position.z = 0.5
p.setup = self.setup
self.add_primitive(p)
def global_menu_reload_cb(self, feedback):
self.reload()
def global_menu_clear_all_cb(self, feedback):
self.clear_all(permanent=False)
def global_menu_clear_all_perm_cb(self, feedback):
self.clear_all()
def menu_save_cb(self, f):
self.save_primitive(f.marker_name)
def menu_scaleable_cb(self, f):
handle = f.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# TODO
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
# TODO
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_moveable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# if feedback.marker_name in self.moveable:
# self.moveable.remove(feedback.marker_name)
self.im_server.erase(feedback.marker_name)
self.im_server.insert(
make_def(self.artificial_objects[feedback.marker_name]),
self.process_im_feedback)
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.make_interactive(
self.artificial_objects[feedback.marker_name])
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_remove_cb(self, feedback):
self.remove_name(feedback.marker_name)
def process_im_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
ps = PoseStamped()
ps.header = feedback.header
ps.pose = feedback.pose
if feedback.marker_name in self.artificial_objects:
self.set_primitive_pose(feedback.marker_name, ps)
elif feedback.marker_name:
# detected objects
pass
def make_interactive(self, p):
im = self.im_server.get(p.name)
for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0,
1))):
im.controls.append(rotate_axis_control("rotate_" + v[0], v[1]))
im.controls.append(move_axis_control("move_" + v[0], v[1]))
def remove_name(self, name):
super(IntCollisionEnv, self).remove_name(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def add_primitive(self, p):
super(IntCollisionEnv, self).add_primitive(p)
self.im_server.insert(make_def(p), self.process_im_feedback)
self.a_obj_menu_handler.apply(self.im_server, p.name)
self.im_server.applyChanges()
def reload(self):
self.im_server.clear()
self.create_menus()
super(IntCollisionEnv, self).reload()
def add_detected(self, name, ps, object_type):
super(IntCollisionEnv, self).add_detected(name, ps, object_type)
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = object_type.bbox
self.im_server.insert(make_def(p, color=(0, 0, 1)),
self.process_im_feedback)
self.im_server.applyChanges()
def clear_detected(self, name):
super(IntCollisionEnv, self).clear_detected(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def check_attached_timer_cb(self, evt):
for name, ps, bbox in self.get_attached():
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = bbox
self.im_server.insert(make_def(p, color=(1, 0, 0)),
self.process_im_feedback)
self.im_server.applyChanges()
