def __init__(self,
root,
name,
description,
server,
ns="",
position0=[0., 0., 0.]):
# Make interactive marker
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = root
self.int_marker.pose.position = Point(*position0)
self.int_marker.scale = 0.1
self.int_marker.name = name
self.name = name
self.int_marker.description = description
# Make the way it should look
mrkrmsg = Marker()
mrkrmsg.type = Marker.CUBE
mrkrmsg.scale.x = 0.03
mrkrmsg.scale.y = 0.03
mrkrmsg.scale.z = 0.03
mrkrmsg.color.r = 0.7
mrkrmsg.color.a = 1.0
# Make the Inner cube control mode:
cubecntrl = InteractiveMarkerControl()
cubecntrl.always_visible = True
cubecntrl.interaction_mode = InteractiveMarkerControl.MOVE_3D
cubecntrl.markers.append(mrkrmsg)
controls = [cubecntrl]
# Make XYZ linear motion control modes:
for i in range(3):
direction = [0] * i + [1] + [0] * (2 - i)
control = InteractiveMarkerControl()
control.name = "move_" + chr(ord("x") + i)
control.orientation = Quaternion(*(direction + [1]))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
controls.append(control)
# Make XYZ rotation control modes:
for i in range(3):
direction = [0] * i + [1] + [0] * (2 - i)
control = InteractiveMarkerControl()
control.name = "rotate_" + chr(ord("x") + i)
control.orientation = Quaternion(*(direction + [1]))
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
controls.append(control)
# Add them to the interactive marker
self.int_marker.controls = controls
# Setup pose publisher
self.pose_pub = rospy.Publisher(ns + name, Pose, queue_size=1)
# Setup the interactive marker in the server
self.server = server
self.server.insert(self.int_marker, self.callbackPublish)
self.server.applyChanges()
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 _CreateMarkerControl(name, orientation, marker_type):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.interaction_mode = marker_type
control.always_visible = False
return control
def _CreateMarkerControl(name, orientation, marker_type):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.interaction_mode = marker_type
control.always_visible = False
return control
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(
current_marker).always_visible
visual.interaction_mode = GetVisualControl(
current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in [
'TranslateXY', 'TranslateYZ', 'TranslateXZ'
]:
new_marker.controls.append(
CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(current_marker).always_visible
visual.interaction_mode = GetVisualControl(current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']:
new_marker.controls.append(CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def make_6dof_controls():
# Edit axes.
# N.B. Follow order from tutorial?
n = 1 / math.sqrt(2)
axes = [
('x', assign(Quaternion(), w=n, x=n, y=0, z=0)),
('z', assign(Quaternion(), w=n, x=0, y=0, z=n)),
('y', assign(Quaternion(), w=n, x=0, y=n, z=0)),
]
controls = []
for name, quat in axes:
control = InteractiveMarkerControl()
control.orientation = quat
control.name = "rotate_" + name
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
controls.append(control)
control = InteractiveMarkerControl()
control.orientation = quat
control.name = "move_" + name
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
controls.append(control)
return controls
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 _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 create_6dof_marker(self):
imarker = InteractiveMarker()
imarker.header.frame_id = self.frame_id
imarker.pose.orientation.w = 1
imarker.name = "tool_calib"
imarker.name = "Tool Calibration"
imarker.scale = 0.2
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
return imarker
def make_marker_flexible(fixed, ps, scale, color, mtype,
ignore_rotation, ignore_x=False,
ignore_y=False, ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'simple_6dof'
int_marker.description = ''
# insert a marker
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(make_marker(scale, color, mtype))
int_marker.controls.append(control)
if fixed:
int_marker.name += '_fixed'
int_marker.description += '\n(fixed orientation)'
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def make_6dof_gripper(fixed, ps, scale, color, robot_type = "pr2",
ignore_rotation = False, ignore_x=False,
ignore_y=False, ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'gripper_6dof'
control = InteractiveMarkerControl()
control.always_visible = True
control.name = 'pr2_gripper_control'
if robot_type == "pr2":
control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control)
int_marker.description = 'pr2_gripper_control'
elif robot_type == "cody":
control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'cody_ee_control'
elif robot_type == "darci":
control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'darci_ee_control'
int_marker.controls.append( control )
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def set_marker_settings(self, arrows, frame=None, scale=0.25):
'''arrows is a list with any number of the following strings: ["x", "y", "z", "a", "b", "c"].'''
if frame:
style = frame.style
else:
style = "none"
## Marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = ""
int_marker.name = ""
int_marker.description = "Frame Editor"
int_marker.pose = Pose()
int_marker.scale = scale
if "x" in arrows:
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation = NewQuaternion(1, 0, 0, 1)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
if "y" in arrows:
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation = NewQuaternion(0, 1, 0, 1)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
if "z" in arrows:
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation = NewQuaternion(0, 0, 1, 1)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
if "a" in arrows:
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation = NewQuaternion(1, 0, 0, 1)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if "b" in arrows:
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation = NewQuaternion(0, 1, 0, 1)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if "c" in arrows:
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.orientation = NewQuaternion(0, 0, 1, 1)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
## Style ##
##
#style_marker = Marker()
#style_marker.scale = NewVector3(0.75*scale, 0.75*scale, 0.75*scale)
#style_marker.color = NewColor(0.0, 0.5, 0.5, 0.75)
#if style != "none":
# style_marker = frame.marker
#style_control = InteractiveMarkerControl()
#style_control.always_visible = True
#style_control.markers.append(style_marker)
#if style != "none":
# int_marker.controls.append(style_control)
self.int_marker = int_marker
def make_marker_flexible(fixed,
ps,
scale,
color,
mtype,
ignore_rotation,
ignore_x=False,
ignore_y=False,
ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'simple_6dof'
int_marker.description = ''
# insert a marker
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(make_marker(scale, color, mtype))
int_marker.controls.append(control)
if fixed:
int_marker.name += '_fixed'
int_marker.description += '\n(fixed orientation)'
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def make_6dof_gripper(fixed,
ps,
scale,
color,
robot_type="pr2",
ignore_rotation=False,
ignore_x=False,
ignore_y=False,
ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'gripper_6dof'
control = InteractiveMarkerControl()
control.always_visible = True
control.name = 'pr2_gripper_control'
if robot_type == "pr2":
control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7],
control=control)
int_marker.description = 'pr2_gripper_control'
elif robot_type == "cody":
control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'cody_ee_control'
elif robot_type == "darci":
control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'darci_ee_control'
int_marker.controls.append(control)
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
