def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head", callback=self.move_head)
menu_handler.insert("Add position to trajectory",
callback=self.add_point)
menu_handler.insert("Place marker over gripper",
callback=self.place_gripper)
menu_handler.insert("Execute trjectory",
callback=self.execute_trajectory)
menu_handler.insert("Clear trajectory", callback=self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback=self.publish_trajectory)
menu_handler.insert("Move the arm (planning)", callback=self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback=self.collision_free_arm)
menu_handler.insert(
"Move the arm to trajectory start (collision-free)",
callback=self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback=self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head",
callback = self.move_head)
menu_handler.insert("Add position to trajectory",
callback = self.add_point)
menu_handler.insert("Place marker over gripper",
callback = self.place_gripper)
menu_handler.insert("Execute trjectory",
callback = self.execute_trajectory)
menu_handler.insert("Clear trajectory",
callback = self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback = self.publish_trajectory)
menu_handler.insert("Move the arm (planning)",
callback = self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback = self.collision_free_arm)
menu_handler.insert("Move the arm to trajectory start (collision-free)",
callback = self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback = self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
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 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()
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 GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' +
self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(
r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(
l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server,
'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(
from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(
to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr(
'TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(
pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
scale=Vector3(0, 0, 0.03),
text=text,
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 = 'ik_target_marker_' + self.side_prefix
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open = False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
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
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
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 ArmControlMarker:
'''Marker for controlling arm of robot.'''
_im_server = None
def __init__(self, arm):
'''
Args:
arm (Arm): Arm object for the robot's arm.
'''
if ArmControlMarker._im_server is None:
im_server = InteractiveMarkerServer('interactive_arm_control')
ArmControlMarker._im_server = im_server
self._arm = arm
self._name = 'arm'
self._is_control_visible = False
self._menu_handler = None
self._prev_is_reachable = None
self._pose = self._arm.get_ee_state()
self._lock = threading.Lock()
self._current_pose = None
self._menu_control = None
self.update()
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def update(self):
'''Updates marker/arm loop'''
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert('Move gripper here',
callback=self._move_to_cb)
self._menu_handler.insert('Move marker to current gripper pose',
callback=self._move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert('Close gripper',
callback=self._close_gripper_cb)
else:
self._menu_handler.insert('Open gripper',
callback=self._open_gripper_cb)
frame_id = REF_FRAME
pose = self.get_pose()
# if self._marker_moved() or self._menu_control is None:
rospy.loginfo("Marker moved")
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
self._menu_control = menu_control
# 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.pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(self._menu_control)
ArmControlMarker._im_server.insert(int_marker,
self._marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server, self._get_name())
ArmControlMarker._im_server.applyChanges()
def reset(self):
'''Sets marker to current arm pose'''
self.set_new_pose(self._arm.get_ee_state(), is_offset=True)
def destroy(self):
'''Removes marker from the world.'''
ArmControlMarker._im_server.erase(self._get_name())
ArmControlMarker._im_server.applyChanges()
def set_new_pose(self, new_pose, is_offset=False):
'''Changes the pose of the marker to new_pose.
Args:
new_pose (PoseStamped)
'''
self._lock.acquire()
if is_offset:
self._pose = new_pose
else:
self._pose = ArmControlMarker._offset_pose(new_pose, -1)
self._lock.release()
def get_pose(self):
'''Returns the pose of the marker
Returns:
Pose
'''
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
return ArmControlMarker._offset_pose(pose)
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3)
Returns:
PoseStamped
'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return PoseStamped(
Header(frame_id='base_link'),
Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3])))
@staticmethod
def _get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose.
Args:
pose (PoseStamped)
Returns:
Matrix3x3
'''
pp, po = pose.pose.position, pose.pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization.
Args:
pose (PoseStamped): The pose to offset.
constant (int, optional): How much to scale the set offset
by (scales DEFAULT_OFFSET). Defaults to 1.
Returns:
PoseStamped: The offset pose.
'''
transform = ArmControlMarker._get_matrix_from_pose(pose)
offset_array = [constant * DEFAULT_OFFSET, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return ArmControlMarker._get_pose_from_transform(hand_transform)
@staticmethod
def _copy_pose(pose):
'''Copies pose msg
Args:
pose (PoseStamped)
Returns:
PoseStamped
'''
copy = PoseStamped(
Header(frame_id=pose.header.frame_id),
Pose(
Point(pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z),
Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w)))
return copy
@staticmethod
def _pose2array(p):
'''Converts pose to array
Args:
p (PoseStamped)
Returns:
numpy.array
'''
return array((p.pose.position.x, p.pose.position.y, p.pose.position.z,
p.pose.orientation.x, p.pose.orientation.y,
p.pose.orientation.z, p.pose.orientation.w))
@staticmethod
def _is_the_same(pose1, pose2, tol=0.001):
'''Checks if poses are the same within tolerance
Args:
pose1 (Pose)
pose2 (Pose)
tol (float, optional)
Returns:
bool
'''
if pose1 is None or pose2 is None:
return True
diff = pose1 - pose2
dist = linalg.norm(diff)
return dist < tol
@staticmethod
def _make_mesh_marker(color):
'''Creates and returns a mesh marker.
Args:
color (ColorRGBA)
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 = color
return mesh
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _marker_moved(self):
'''Returns whether marker has moved
Returns:
bool
'''
moved = True
current_pose = self.get_pose()
if not self._current_pose is None:
if ArmControlMarker._is_the_same(
ArmControlMarker._pose2array(current_pose),
ArmControlMarker._pose2array(self._current_pose)):
moved = False
self._current_pose = current_pose
return moved
def _marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if 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
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
self.set_new_pose(
PoseStamped(Header(frame_id='base_link'), feedback.pose))
self.update()
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))
def _open_gripper_cb(self, feedback):
'''Callback for opening gripper.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self._arm.open_gripper()
for i in range(30):
if self._arm.get_gripper_state() == GripperState.OPEN:
break
rospy.sleep(0.1)
self.update()
def _close_gripper_cb(self, feedback):
'''Callback for closing gripper.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self._arm.close_gripper()
for i in range(30):
if self._arm.get_gripper_state() != GripperState.OPEN:
break
rospy.sleep(0.1)
self.update()
def _move_to_cb(self, feedback):
'''Callback for when moving to a pose is requested.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
if not ArmControlMarker._is_the_same(
ArmControlMarker._pose2array(self._pose),
ArmControlMarker._pose2array(self._arm.get_ee_state()), 0.01):
rospy.loginfo("Move to cb from marker for real")
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
target_pose = pose
if target_pose is not None:
# time_to_pose = self._arm.get_time_to_pose(self.get_pose())
thread = threading.Thread(group=None,
target=self._arm.move_to_pose,
args=(target_pose, ),
name='move_to_arm_state_thread')
thread.start()
# Log
# side_str = self._arm.side()
rospy.loginfo('Started thread to move arm.')
else:
rospy.loginfo('Will not move arm; unreachable.')
else:
rospy.loginfo("move too small?")
def _move_pose_to_cb(self, feedback):
'''Callback for moving gripper marker to current pose.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self.reset()
self.update()
def _is_reachable(self):
'''Checks and returns whether there is an IK solution for this
marker pose
Returns:
bool: Whether this action step is reachable.
'''
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(pose,
self._arm.get_joint_state())
is_reachable = (target_joints is not None)
# 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(reachable_str)
# Cache and return.
self._prev_is_reachable = is_reachable
print
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 self._name
def _is_hand_open(self):
'''Returns whether the gripper is open
Returns:
bool
'''
return self._arm.get_gripper_state() == GripperState.OPEN
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 the 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 _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.
'''
mesh_color = self._get_mesh_marker_color()
# Create mesh 1 (palm).
mesh1 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose.position.x = DEFAULT_OFFSET
mesh1.pose.orientation.w = 1
# TODO (sarah): make all of these numbers into constants
if is_hand_open:
mesh2 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
mesh2.pose.position.x = 0.08
mesh2.pose.position.y = -0.165
mesh2.pose.orientation.w = 1
mesh3 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
mesh3.pose.position.x = 0.08
mesh3.pose.position.y = 0.165
mesh3.pose.orientation.w = 1
else:
mesh2 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
mesh2.pose.position.x = 0.08
mesh2.pose.position.y = -0.116
mesh2.pose.orientation.w = 1
mesh3 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
mesh3.pose.position.x = 0.08
mesh3.pose.position.y = 0.116
mesh3.pose.orientation.w = 1
# Append all meshes we made.
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
# Return back the control.
# TODO(mbforbes): Why do we do this?
return control
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except Exception as e:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
import traceback
traceback.print_exc()
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
self.move_to_joints(self.side_prefix, [ik_sol], 1.0)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def marker_cb(self, pose_markers):
rospy.loginfo('AR Marker Pose updating')
transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
offset_array = [0, 0, .03]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
self.update_viz()
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = "x=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
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))))
label_pos = Point()
label_pos.x = pose.position.x
label_pos.y = pose.position.y
label_pos.z = pose.position.z
label = "{} arm".format(self.side_prefix)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=label,
color=ColorRGBA(0.0, 0.0, 0.0, 0.9),
header=Header(frame_id=frame_id),
pose=Pose(label_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [-GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
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
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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 move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move += time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
class InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame + " at " +
str(position.x) + " " + str(position.y) + " " +
str(position.z) + " and orientation " + str(o.x) + " " +
str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped_obj',
PoseStamped,
queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
pos = ModelState()
pos.model_name = self.to_frame
pos.pose = self.last_pose
pos.reference_frame = self.from_frame
pub.publish(pos)
def print_commands(self, pose, decimals=4):
rospy.Subscriber("/posestamped", PoseStamped, callback, pose)
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi / 2.0
p = pi
y = pi / 2.0
o = quaternion_from_euler(r, p, y)
opt_frame = self.to_frame + "_rgb_optical_frame"
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), self.to_frame + "_link", self.from_frame)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
br = tf.TransformBroadcaster()
br.sendTransform((0, 0, 0), o, rospy.Time.now(), opt_frame,
self.to_frame + "_link")
r.sleep()
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory '
+ 'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory '
+ 'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr('TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
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 = 'ik_target_marker_' + self.side_prefix
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
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
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
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 InteractiveMarkerPlugin(PluginBase):
"""
Spawns interactive Marker which send cart goals to action server.
Does not interact with god map.
"""
def __init__(self, root_tips, suffix=u''):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.markers = {}
super(InteractiveMarkerPlugin, self).__init__()
def start_once(self):
# giskard goal client
self.client = SimpleActionClient(u'qp_controller/command', MoveAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
all_goals = {}
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name,
self.client, root, tip, all_goals))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def make_sphere(self, msg):
"""
:param msg:
:return:
:rtype: Marker
"""
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * MARKER_SCALE * 2
marker.scale.y = msg.scale * MARKER_SCALE * 2
marker.scale.z = msg.scale * MARKER_SCALE * 2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def make_sphere_control(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.make_sphere(msg))
msg.controls.append(control)
return control
def make_6dof_marker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.scale = MARKER_SCALE
int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)
# insert a box
self.make_sphere_control(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
}
int_marker.name += u'_' + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.markers[int_marker.name] = int_marker
return int_marker
class process_feedback(object):
def __init__(self, i_server, marker_name, client, root_link, tip_link,
all_goals):
"""
:param i_server:
:type i_server: InteractiveMarkerServer
:param marker_name:
:type marker_name: str
:param client:
:type client: SimpleActionClient
:param root_link:
:type root_link: str
:param tip_link:
:type tip_link: str
:param all_goals:
:type all_goals: dict
"""
self.i_server = i_server
self.marker_name = marker_name
self.client = client
self.tip_link = tip_link
self.root_link = root_link
self.all_goals = all_goals
self.reset_goal()
self.marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
def reset_goal(self):
p = Pose()
p.orientation.w = 1
self.all_goals[self.root_link, self.tip_link] = []
self.all_goals[self.root_link, self.tip_link].append(
self.make_translation_controller(self.tip_link, p))
self.all_goals[self.root_link, self.tip_link].append(
self.make_rotation_controller(self.tip_link, p))
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
self.all_goals = defaultdict(list)
self.all_goals[self.root_link, self.tip_link] = []
print(u'got interactive goal update')
# translation
controller = self.make_translation_controller(
feedback.header.frame_id, feedback.pose)
self.all_goals[self.root_link,
self.tip_link].append(controller)
# rotation
controller = self.make_rotation_controller(
feedback.header.frame_id, feedback.pose)
self.all_goals[self.root_link,
self.tip_link].append(controller)
self.send_all_goals()
self.pub_goal_marker(feedback.header, feedback.pose)
self.reset_goal()
self.i_server.setPose(self.marker_name, Pose())
self.i_server.applyChanges()
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)
def make_translation_controller(self, frame_id, pose):
"""
:param frame_id:
:type frame_id: str
:param pose:
:type pose: Pose
:return:
:rtype: giskard_msgs.msg._Controller.Controller
"""
controller = self.make_controller(frame_id, pose)
controller.type = Controller.TRANSLATION_3D
controller.p_gain = 3
controller.max_speed = 0.3
controller.weight = 1.0
return controller
def make_rotation_controller(self, frame_id, pose):
controller = self.make_controller(frame_id, pose)
controller.type = Controller.ROTATION_3D
controller.p_gain = 3
controller.max_speed = 0.5
controller.weight = 1.0
return controller
def make_controller(self, frame_id, pose):
controller = Controller()
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header.frame_id = frame_id
controller.goal_pose.pose = pose
return controller
def send_all_goals(self):
goal = MoveGoal()
goal.type = MoveGoal.PLAN_AND_EXECUTE
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = 20
for g in self.all_goals.values():
move_cmd.controllers.extend(g)
goal.cmd_seq.append(move_cmd)
self.client.send_goal(goal)
def stop(self):
self.marker_pub.unregister()
def copy(self):
return self
class IMServer(object):
"""
Spawns interactive Marker which send cart goals to action server.
Does not interact with god map.
"""
def __init__(self,
root_tips,
suffix=u'',
giskard_name=u'giskardpy/command'):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
self.enable_self_collision = rospy.get_param(u'~enable_self_collision',
True)
self.giskard = GiskardWrapper(giskard_name)
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.giskard_name = giskard_name
self.markers = {}
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name, root, tip,
self.giskard,
self.enable_self_collision))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def make_sphere(self, msg):
"""
:param msg:
:return:
:rtype: Marker
"""
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * MARKER_SCALE * 2
marker.scale.y = msg.scale * MARKER_SCALE * 2
marker.scale.z = msg.scale * MARKER_SCALE * 2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def make_sphere_control(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.make_sphere(msg))
msg.controls.append(control)
return control
def make_6dof_marker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.scale = MARKER_SCALE
int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)
# insert a box
self.make_sphere_control(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
}
int_marker.name += u'_' + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.markers[int_marker.name] = int_marker
return int_marker
class process_feedback(object):
def __init__(self, i_server, marker_name, root_link, tip_link, giskard,
enable_self_collision):
"""
:param i_server:
:type i_server: InteractiveMarkerServer
:param marker_name:
:type marker_name: str
:param client:
:type client: SimpleActionClient
:param root_link:
:type root_link: str
:param tip_link:
:type tip_link: str
:param giskard:
:type giskard: GiskardWrapper
:param enable_self_collision:
:type enable_self_collision: bool
"""
self.i_server = i_server
self.marker_name = marker_name
self.tip_link = tip_link
self.root_link = root_link
self.giskard = giskard
self.enable_self_collision = enable_self_collision
self.marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
logging.loginfo(u'got interactive goal update')
p = PoseStamped()
p.header.frame_id = feedback.header.frame_id
p.pose = feedback.pose
self.giskard.set_cart_goal(self.root_link, self.tip_link, p)
if not self.enable_self_collision:
self.giskard.allow_self_collision()
self.giskard.plan_and_execute(wait=False)
self.pub_goal_marker(feedback.header, feedback.pose)
self.i_server.setPose(self.marker_name, Pose())
self.i_server.applyChanges()
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)
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 InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame + " at " +
str(position.x) + " " + str(position.y) + " " +
str(position.z) + " and orientation " + str(o.x) + " " +
str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(
p.y, decimals)) + " " + str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(
p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(
o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(
round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi / 2.0
p = pi
y = pi / 2.0
o = quaternion_from_euler(r, p, y)
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), self.to_frame, self.from_frame)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
r.sleep()
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)
class GripperMarkers:
_offset = -0.09
_tf_listener = None
def __init__(self, side_prefix):
if GripperMarkers._tf_listener is None:
GripperMarkers._tf_listener = TransformListener()
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers')
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
GripperMarkers._tf_listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(5))
t = GripperMarkers._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = GripperMarkers._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
rospy.logwarn('No IK solutions for this pose, cannot move.')
else:
self.ik.move_to_joints(ik_solution, 2.0)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.marker_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.marker_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
#text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
text = self.side_prefix + '_arm'
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.05),
text=text,
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 = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
offset = -GripperMarkers._offset
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - 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 = -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 = GripperMarkers.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 = GripperMarkers.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 - 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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose, x_offset):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [x_offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
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
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
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()
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
#TODO: Send the arms to ik_solution
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
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 = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
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
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
class InteractiveMarkerPoseStampedPublisher():
def __init__(self):
self.server = InteractiveMarkerServer("posestamped_im")
self.menu_handler = MenuHandler()
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1)
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1)
self.pub = rospy.Publisher('/pose_to_follow',
PoseStamped,
queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
#self.global_frame_id = 'world'
self.global_frame_id = 'base_footprint'
pose = Pose()
pose.position.x = 0.2
# pose.position.y = 0.35
pose.position.y = -0.35
#pose.position.z = 0.1
pose.position.z = 1.1
pose.orientation.w = 1.0
# pose.orientation.x = 0.35762
# pose.orientation.y = 0.50398
# pose.orientation.z = 0.45213
# pose.orientation.w = 0.64319
#self.makeMenuMarker(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.loginfo(s + ": pose changed")
ps = PoseStamped()
ps.header.frame_id = self.global_frame_id #'/world'
ps.pose = feedback.pose
self.pub.publish(ps)
# TODO
# << "\nposition = "
# << feedback.pose.position.x
# << ", " << feedback.pose.position.y
# << ", " << feedback.pose.position.z
# << "\norientation = "
# << feedback.pose.orientation.w
# << ", " << feedback.pose.orientation.x
# << ", " << feedback.pose.orientation.y
# << ", " << feedback.pose.orientation.z
# << "\nframe: " << feedback.header.frame_id
# << " time: " << feedback.header.stamp.sec << "sec, "
# << feedback.header.stamp.nsec << " nsec" )
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.loginfo(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.loginfo(s + ": mouse up" + mp + ".")
self.server.applyChanges()
def makeArrow(self, msg):
marker = Marker()
marker.type = Marker.ARROW
marker.scale.x = 0.15 #msg.scale * 0.3
marker.scale.y = 0.08 #msg.scale * 0.1
marker.scale.z = 0.03 #msg.scale * 0.03
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame_id # "/world"
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "" #"EEF 6DOF control"
# insert a box, well, an arrow
self.makeBoxControl(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)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
self.menu_handler.insert("Do stuff", callback=self.processFeedback)
## This makes the floating text appear
# make one control using default visuals
# control = InteractiveMarkerControl()
# control.interaction_mode = InteractiveMarkerControl.MENU
# control.description="Menu"
# control.name = "menu_only_control"
# int_marker.controls.append(copy.deepcopy(control))
# marker = self.makeArrow( int_marker )
# control.markers.append( marker )
# control.always_visible = False
# int_marker.controls.append(control)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
class InteractiveMarkerGoal(object):
def __init__(self, root_link, tip_links):
# tf
self.tfBuffer = Buffer(rospy.Duration(1))
self.tf_listener = TransformListener(self.tfBuffer)
# giskard goal client
# self.client = SimpleActionClient('move', ControllerListAction)
self.client = SimpleActionClient('qp_controller/command',
ControllerListAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer("eef_control")
self.menu_handler = MenuHandler()
for tip_link in tip_links:
int_marker = self.make6DofMarker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
self.server.insert(
int_marker,
self.process_feedback(self.server, self.client, root_link,
tip_link))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def transformPose(self, target_frame, pose, time=None):
transform = self.tfBuffer.lookup_transform(
target_frame, pose.header.frame_id,
pose.header.stamp if time is not None else rospy.Time(0),
rospy.Duration(1.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
def makeBox(self, msg):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.2
marker.scale.y = msg.scale * 0.2
marker.scale.z = msg.scale * 0.2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeBox(msg))
msg.controls.append(control)
return control
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
class process_feedback(object):
def __init__(self, i_server, client, root_link, tip_link):
self.i_server = i_server
self.client = client
self.tip_link = tip_link
self.root_link = root_link
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print('interactive goal update')
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# asd = 'asd'
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
self.i_server.applyChanges()
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if(joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
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 = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
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 = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
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 ArmControlMarker:
'''Marker for visualizing the steps of an action.'''
_im_server = None
def __init__(self, arm):
'''
Args:
arm (Arm): Arm object for the right or left PR2 arm.
'''
if ArmControlMarker._im_server is None:
im_server = InteractiveMarkerServer('interactive_arm_control')
ArmControlMarker._im_server = im_server
self._arm = arm
self._is_control_visible = False
self._menu_handler = None
self._prev_is_reachable = None
self._pose = self._arm.get_ee_state()
self._lock = threading.Lock()
def update(self):
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert(
'Move gripper here', callback=self.move_to_cb)
self._menu_handler.insert(
'Move marker to current gripper pose',
callback=self.move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert(
'Close gripper',
callback=self.close_gripper_cb)
else:
self._menu_handler.insert(
'Open gripper',
callback=self.open_gripper_cb)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = REF_FRAME
pose = self.get_pose()
menu_control = self._make_gripper_marker(
menu_control, self._is_hand_open())
# 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)
ArmControlMarker._im_server.insert(
int_marker, self.marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server,
self._get_name())
ArmControlMarker._im_server.applyChanges()
def reset(self):
self.set_new_pose(self._arm.get_ee_state(), is_offset=True)
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3): (I think this is the correct type.
See ActionStepMarker as a reference for how to use.)
Returns:
Pose
'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(
Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3])
)
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose.
Args:
pose (Pose)
Returns:
Matrix3x3: (I think this is the correct type. See
ActionStepMarker as a reference for how to use.)
'''
pp, po = pose.position, pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@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 DEFAULT_OFFSET). Defaults to 1.
Returns:
Pose: The offset pose.
'''
transform = ArmControlMarker.get_matrix_from_pose(pose)
offset_array = [constant * DEFAULT_OFFSET, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return ArmControlMarker.get_pose_from_transform(hand_transform)
def get_uid(self):
'''Returns a unique id for this marker.
Returns:
int: A number that is unique given the arm
index.
'''
return self._arm.arm_index
def destroy(self):
'''Removes marker from the world.'''
ArmControlMarker._im_server.erase(self._get_name())
ArmControlMarker._im_server.applyChanges()
def set_new_pose(self, new_pose, is_offset=False):
'''Changes the pose of the action step to new_pose.
Args:
new_pose (Pose)
'''
self._lock.acquire()
if is_offset:
self._pose = new_pose
else:
self._pose = ArmControlMarker._offset_pose(new_pose, -1)
self._lock.release()
@staticmethod
def copy_pose(pose):
copy = Pose(
Point(pose.position.x, pose.position.y, pose.position.z),
Quaternion(pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
)
return copy
def get_pose(self):
'''Returns the pose of the action step.
Returns:
Pose
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
return ArmControlMarker._offset_pose(pose)
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)
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
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))
def open_gripper_cb(self, __):
self._arm.open_gripper()
def close_gripper_cb(self, __):
self._arm.close_gripper()
def move_to_cb(self, __):
'''Callback for when moving to a pose is requested.
Args:
__ (???): Unused
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(
pose, self._arm.get_joint_state())
if target_joints is not None:
time_to_pose = self._arm.get_time_to_pose(self.get_pose())
thread = threading.Thread(
group=None,
target=self._arm.move_to_joints,
args=(target_joints, time_to_pose),
name='move_to_arm_state_thread'
)
thread.start()
# Log
side_str = self._arm.side()
rospy.loginfo('Started thread to move ' + side_str + ' arm.')
else:
rospy.loginfo('Will not move ' + side_str + ' arm; unreachable.')
def move_pose_to_cb(self, __):
'''Callback for when a pose change to current is requested.
Args:
__ (???): Unused
'''
self.reset()
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.
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(
pose, self._arm.get_joint_state())
is_reachable = (target_joints is not None)
# 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(self._arm.side() + ':' + 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 'arm' + str(self._arm.arm_index)
def _is_hand_open(self):
'''Returns whether the gripper is open for this action step.
Returns:
bool
'''
return self._arm.get_gripper_state() == GripperState.OPEN
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 = GRIPPER_MARKER_SCALE
mesh.scale.y = GRIPPER_MARKER_SCALE
mesh.scale.z = GRIPPER_MARKER_SCALE
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 - DEFAULT_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 = -DEFAULT_OFFSET
mesh1.pose.orientation.w = 1
# Create mesh 2 (finger).
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = ArmControlMarker.get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = ArmControlMarker.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 - DEFAULT_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 = ArmControlMarker.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 = ArmControlMarker.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 GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos,
rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if (joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(
pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
scale=Vector3(0, 0, 0.03),
text=text,
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 = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open = False
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 - GripperMarkers._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 = -GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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 - GripperMarkers._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 = GripperMarkers.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 = GripperMarkers.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
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
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 = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
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):
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
class InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame +
" at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
" and orientation " + str(o.x) + " " + str(o.y) +
" " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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
control.orientation_mode = InteractiveMarkerControl.FIXED
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_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi/2.0
p = pi
y = pi/2.0
o = quaternion_from_euler(r, p, y)
opt_frame = self.to_frame + "_rgb_optical_frame"
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z),
(ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(),
self.to_frame + "_link",
self.from_frame
)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
br = tf.TransformBroadcaster()
br.sendTransform((0, 0, 0),
o,
rospy.Time.now(),
opt_frame,
self.to_frame + "_link")
r.sleep()
