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 _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def CreateInteractiveMarker(frame_id, name, scale):
interactive_marker = InteractiveMarker()
interactive_marker.header.frame_id = frame_id
interactive_marker.name = name
interactive_marker.description = name
interactive_marker.scale = scale
return interactive_marker
def makeHumanTagMarker(self, position, name):
"""
Make coordinates and control for tag
:param: position of tag
:param: name for tag
:returns:
"""
int_marker = InteractiveMarker()
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = name
int_marker.description = name
self.makeBoxControlHumanTag(int_marker)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, self.processFeedback)
def create_interactive_marker(self, T):
im = InteractiveMarker()
im.header.frame_id = "world"
im.name = "target"
im.description = "Controller Target"
im.scale = 0.2
im.pose.position = Point(*T[0:3, 3])
im.pose.orientation = Quaternion(*tf.quaternion_from_matrix(T))
self.process_marker_feedback(im) # set target to initial pose
# Create a control to move a (sphere) marker around with the mouse
control = InteractiveMarkerControl()
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.markers.extend(
frame(numpy.identity(4), scale=0.1, frame_id='').markers)
im.controls.append(control)
# Create arrow controls to move the marker
for dir in 'xyz':
control = InteractiveMarkerControl()
control.name = "move_" + dir
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation.x = 1 if dir == 'x' else 0
control.orientation.y = 1 if dir == 'y' else 0
control.orientation.z = 1 if dir == 'z' else 0
control.orientation.w = 1
im.controls.append(control)
# Add the marker to the server and indicate that processMarkerFeedback should be called
self.insert(im, self.process_marker_feedback)
# Publish all changes
self.applyChanges()
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 _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def make_marker(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.pose = self.pose
int_marker.scale = 1
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
int_marker.controls.append(copy.deepcopy(control))
# make a box which also moves in the plane
markers = self.make_individual_markers(int_marker)
for marker in markers:
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(int_marker, self.feedback)
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_):
int_marker = InteractiveMarker()
int_marker.header.frame_id = revolute_joint.child_body().body_frame().name()
int_marker.name = revolute_joint.name()
int_marker.scale = 0.3
int_marker.pose.position.x = 0.
int_marker.pose.position.y = 0.
int_marker.pose.position.z = 0.
int_marker.pose.orientation.w = 1.
int_marker.pose.orientation.x = 0.
int_marker.pose.orientation.y = 0.
int_marker.pose.orientation.z = 0.
# Drake revolute axis is in frame F on parent
axis_hat = revolute_joint.revolute_axis()
self._joint_axis_in_child[revolute_joint.name()] = axis_hat
# What rotation would get the parent X axis to align with the joint axis?
rotation_matrix = ComputeBasisFromAxis(0, axis_hat)
pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion()
joint_control = InteractiveMarkerControl()
joint_control.orientation.w = pydrake_quat.w()
joint_control.orientation.x = pydrake_quat.x()
joint_control.orientation.y = pydrake_quat.y()
joint_control.orientation.z = pydrake_quat.z()
joint_control.always_visible = True
joint_control.name = f'rotate_axis_{revolute_joint.name()}'
joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(joint_control)
return int_marker
def MakeMuneObject(self, MenuName, MenuPose):
MenuInteractiveMarker = InteractiveMarker()
MenuInteractiveMarker.name = MenuName
MenuInteractiveMarker.header.frame_id = self.frame_id
MenuInteractiveMarker.pose.position.z += self.MenuHight
MenuInteractiveMarker.scale = self.MenuScale
MenuControl = InteractiveMarkerControl()
MenuControl.interaction_mode = InteractiveMarkerControl.MENU
MenuControl.always_visible = False
MenuMarker = Marker()
MenuMarker.type = Marker.ARROW
MenuMarker.scale.x = MenuInteractiveMarker.scale * 2
MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45
MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45
MenuMarker.color.r = 0.5
MenuMarker.color.g = 0.5
MenuMarker.color.b = 0.5
MenuMarker.color.a = 1.0
MenuMarker.pose = MenuPose
MenuControl.markers.append(MenuMarker)
MenuInteractiveMarker.controls.append(MenuControl)
#print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker
self.server.insert(MenuInteractiveMarker)
rospy.loginfo('insert Menu Marker Object')
def CreateInteractiveMarker(frame_id, name, scale):
interactive_marker = InteractiveMarker()
interactive_marker.header.frame_id = frame_id
interactive_marker.name = name
interactive_marker.description = name
interactive_marker.scale = scale
return interactive_marker
def start(self):
# create blue cube marker
obj_marker = Marker()
obj_marker.type = Marker.CUBE
obj_marker.pose.orientation.w = 1
obj_marker.scale.x = 0.06
obj_marker.scale.y = 0.06
obj_marker.scale.z = 0.06
obj_marker.color.r = 0.09
obj_marker.color.g = 0.97
obj_marker.color.b = 2.39
obj_marker.color.a = 1.0
obj_control = InteractiveMarkerControl()
obj_control.interaction_mode = InteractiveMarkerControl.MENU
obj_control.always_visible = True
obj_control.markers.append(obj_marker)
obj_im = InteractiveMarker()
obj_im.name = 'object'
obj_im.header.frame_id = 'base_link'
obj_im.pose.position.x = 0.7
obj_im.pose.position.y = 0
obj_im.pose.position.z = 0.5
obj_im.pose.orientation.w = 1
obj_im.scale = 0.3
obj_im.menu_entries.append(menu_entry(1, 'Pick from front'))
obj_im.menu_entries.append(menu_entry(2, 'Open gripper'))
obj_im.controls.append(obj_control)
obj_im.controls.extend(six_dof_controls())
self._im_server.insert(obj_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
self.update_gripper()
def make_def(p, color=(0.5, 0.5, 0.5), desc=None):
im = InteractiveMarker()
im.header.frame_id = p.pose.header.frame_id
im.pose = p.pose.pose
im.name = p.name
if desc is None:
im.description = p.name
im.scale = 1.2 * max(p.bbox.dimensions)
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = p.bbox.dimensions[0]
marker.scale.y = p.bbox.dimensions[1]
marker.scale.z = p.bbox.dimensions[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.markers.append(marker)
im.controls.append(control)
return im
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 makeMarker(self,
callback=None,
marker=None,
pose=[0, 0, 0],
controls=[],
fixed=False,
name=None,
frame="map",
description="",
imode=0,
rot=[0, 0, 0, 1]):
if marker is None:
marker = self.mg.marker()
if callback is None:
callback = default_callback
if name is None:
name = "control%d" % self.c
self.c += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position.x = pose[0]
int_marker.pose.position.y = pose[1]
int_marker.pose.position.z = pose[2]
int_marker.pose.orientation.x = rot[0]
int_marker.pose.orientation.y = rot[1]
int_marker.pose.orientation.z = rot[2]
int_marker.pose.orientation.w = rot[3]
int_marker.scale = 1
int_marker.name = name
int_marker.description = description
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = imode
control.markers.append(marker)
int_marker.controls.append(control)
for control_name in controls:
data = TYPEDATA[control_name]
control = InteractiveMarkerControl()
control.orientation.w = data[0] / SQRT2
control.orientation.x = data[1] / SQRT2
control.orientation.y = data[2] / SQRT2
control.orientation.z = data[3] / SQRT2
control.name = control_name
control.interaction_mode = data[4]
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.server.insert(int_marker, callback)
self.markers[name] = int_marker
self.server.applyChanges()
def _get_object_marker(self, index, mesh=None):
"""Generate and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
"""
int_marker = InteractiveMarker()
int_marker.name = self._objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = self._objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=index,
lifetime=MARKER_DURATION,
scale=self._objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=self._objects[index].object.pose)
self.create_sides(index)
if mesh is not None:
object_marker = _get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
else:
for item in range((6 * (index)), (6 * (index)) + 6):
rospy.loginfo("Item: " + str(item) + "\nlen mark cont: " +
str(len(self._marker_controllers)))
int_marker.controls.append(self._marker_controllers[item])
text_pos = Point()
text_pos.x = self._objects[index].object.pose.position.x
text_pos.y = self._objects[index].object.pose.position.y
text_pos.z = (self._objects[index].object.pose.position.z +
self._objects[index].object.dimensions.z / 2 +
OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
#rospy.loginfo("Int_marker controls: " + str(int_marker.controls))
return int_marker
def _get_object_marker(self, index, mesh=None):
'''Generate and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=MARKER_DURATION,
scale=World.objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=World.objects[index].object.pose
)
if mesh is not None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (
World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
int_marker.controls.append(button_control)
return int_marker
def _add_poi(self, name, position):
'''
Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change
'''
if self.interactive_marker_server.get(name) is not None:
return False
poi = POI()
poi.name = name
poi.position = position
self.pois.pois.append(poi)
point_marker = Marker()
point_marker.type = Marker.SPHERE
point_marker.scale.x = self.marker_scale
point_marker.scale.y = self.marker_scale
point_marker.scale.z = self.marker_scale
point_marker.color.r = 1.0
point_marker.color.g = 1.0
point_marker.color.b = 1.0
point_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1.0
text_marker.pose.position.x = 1.5
text_marker.text = poi.name
text_marker.scale.z = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame
int_marker.pose.orientation.w = 1.0
int_marker.pose.position = poi.position
int_marker.scale = 1
int_marker.name = poi.name
# insert a box
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.always_visible = True
control.markers.append(point_marker)
control.markers.append(text_marker)
int_marker.controls.append(control)
self.interactive_marker_server.insert(int_marker,
self.process_feedback)
return True
def __init__(self, arm, gripper, im_server):
self.arm = arm
self.gripper = gripper
self.currentPose = None
self.currentPoseFeasible = False
self._im_server = im_server
gripper_im = InteractiveMarker()
gripper_im.name = "da grip"
#gripper_im.header.frame_id = "wrist_roll_link"
gripper_im.header.frame_id = "base_link"
gripper_im.scale = .5
gripper_im.pose.orientation.w = 1.0
markers = init_markers(0.166, 0, 0) # create gripper mesh
# Adds the gripper markers to the interactive markers
marker_control = InteractiveMarkerControl()
for marker in markers:
marker_control.markers.append(marker)
marker_control.always_visible = True
marker_control.interaction_mode = InteractiveMarkerControl.MENU
gripper_im.controls.append(marker_control)
# Add the 6 degree of freedom controls
add_6dof_controls(gripper_im)
# Add the menu commands
entry1 = MenuEntry()
entry1.id = 1
entry1.title = 'open'
entry1.command_type = entry1.FEEDBACK
gripper_im.menu_entries.append(entry1)
entry2 = MenuEntry()
entry2.id = 2
entry2.title = 'close'
entry2.command_type = entry2.FEEDBACK
gripper_im.menu_entries.append(entry2)
entry3 = MenuEntry()
entry3.id = 3
entry3.title = 'move_gripper'
entry3.command_type = entry3.FEEDBACK
gripper_im.menu_entries.append(entry3)
self._im_server.insert(gripper_im, feedback_cb = self.handle_feedback)
self._im_server.applyChanges()
def createInteractiveMarker(name, x=0, y=0, z=0, ox=0, oy=0, oz=0, ow=1, frame_id = "/map"):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = 0.3
int_marker.description = name
int_marker.pose.position.x = x
int_marker.pose.position.y = y
int_marker.pose.position.z = z
int_marker.pose.orientation.x = ox
int_marker.pose.orientation.y = oy
int_marker.pose.orientation.z = oz
int_marker.pose.orientation.w = ow
return int_marker
def createInteractiveMarker(name, x=0, y=0, z=0, ox=0, oy=0, oz=0, ow=1, frame_id = "/map"):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = 0.3
int_marker.description = name
int_marker.pose.position.x = x
int_marker.pose.position.y = y
int_marker.pose.position.z = z
int_marker.pose.orientation.x = ox
int_marker.pose.orientation.y = oy
int_marker.pose.orientation.z = oz
int_marker.pose.orientation.w = ow
return int_marker
def add_marker(self,
init_position,
callback,
controls=[],
scale=1.,
marker=None,
name=None,
fixed_orientation=False):
if name is None:
name = 'control%d' % (self.marker_ctr)
self.marker_ctr += 1
if marker is None:
marker = make_markers.make_marker()
marker.header.frame_id = self.frame_id
marker.header.frame_id = self.frame_id
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.pose.position = init_position
int_marker.scale = scale
int_marker.name = name
vm_control = InteractiveMarkerControl()
vm_control.always_visible = True
vm_control.markers.append(marker)
int_marker.controls.append(vm_control)
#int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
for control_name in controls:
data = TYPEDATA[control_name]
control = InteractiveMarkerControl()
control.orientation.w = data[0]
control.orientation.x = data[1]
control.orientation.y = data[2]
control.orientation.z = data[3]
control.name = control_name
control.interaction_mode = data[4]
if fixed_orientation:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.server.insert(int_marker, callback)
self.markers[name] = int_marker
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 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 _get_surface_marker(pose, dimensions):
'''Returns a surface marker with provided pose and dimensions.
Args:
pose (Pose)
dimensions (Vector3)
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = BASE_LINK
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=MARKER_DURATION,
scale=dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_SURFACE,
pose=pose
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=SCALE_TEXT, text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def interactive_gripper_marker(pose_stamped, finger_distance):
# gripper marker
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000]
gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(CLOSE_FINGER_POS,
min(finger_distance, OPEN_FINGER_POS))
# left finger marker
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = -finger_distance / 2.0
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
# right finger marker
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = finger_distance / 2.0
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.3
return interactive_marker
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_):
int_marker = InteractiveMarker()
int_marker.header.frame_id = revolute_joint.child_body().body_frame().name()
int_marker.name = revolute_joint.name()
int_marker.scale = 0.3
int_marker.pose.position.x = 0.
int_marker.pose.position.y = 0.
int_marker.pose.position.z = 0.
int_marker.pose.orientation.w = 1.
int_marker.pose.orientation.x = 0.
int_marker.pose.orientation.y = 0.
int_marker.pose.orientation.z = 0.
# Drake revolute axis is in frame F on parent
axis_hat = revolute_joint.revolute_axis()
self._joint_axis_in_child[revolute_joint.name()] = axis_hat
# What rotation would get the parent X axis to align with the joint axis?
# https://math.stackexchange.com/q/476311
x_axis = (1, 0, 0)
v = numpy.cross(x_axis, axis_hat)
c = numpy.dot(x_axis, axis_hat)
v_sub_x = numpy.array((
(0, -v[2], v[1]),
(v[2], 0, -v[0]),
(-v[1], v[0], 0)), dtype=numpy.float64)
v_sub_x_squared = numpy.dot(v_sub_x, v_sub_x)
rotation_matrix = numpy.eye(3) + v_sub_x + v_sub_x_squared * (1.0 / (1.0 + c))
pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion()
joint_control = InteractiveMarkerControl()
joint_control.orientation.w = pydrake_quat.w()
joint_control.orientation.x = pydrake_quat.x()
joint_control.orientation.y = pydrake_quat.y()
joint_control.orientation.z = pydrake_quat.z()
joint_control.always_visible = True
joint_control.name = f'rotate_axis_{revolute_joint.name()}'
joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(joint_control)
return int_marker
def __init__(self, server, x, y, name, driver):
# ... Initialization, marker creation, etc. ...
self._driver = driver
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.name = name
int_marker.description = name
int_marker.pose.position.x = x
int_marker.pose.position.y = y
int_marker.pose.orientation.w = 1
int_marker.scale = 1.1
# print name
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.7
arrow_marker.scale.y = 0.07
arrow_marker.scale.z = 0.07
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.always_visible = True
control.markers.append(arrow_marker)
int_marker.controls.append(control)
#int_marker_control.markers.append(arrow_marker)
self._server = server
self._server.insert(int_marker, self._callback)
self._server.applyChanges()
def build_landmark_marker(landmark):
"""Generate and return a marker for world landmarks.
Args:
landmark (WorldLandmark): The landmark to generate a marker for.
Returns:
InteractiveMarker
"""
int_marker = InteractiveMarker()
int_marker.name = landmark.name()
int_marker.header.frame_id = BASE_LINK
int_marker.pose = landmark.object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
lifetime=MARKER_DURATION,
scale=landmark.object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=landmark.object.pose)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = landmark.object.pose.position.x
text_pos.y = landmark.object.pose.position.y
text_pos.z = (landmark.object.pose.position.z +
landmark.object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def gripper_interactive_marker(pose_stamped, finger_distance):
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166
gripper_marker.pose.orientation.w = 1
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(0, min(finger_distance, 0.1))
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = finger_distance / 2.0 - 0.1
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = -finger_distance / 2.0 + 0.1
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.25
return interactive_marker
def make_waypoint_marker(id, x, y):
int_marker = InteractiveMarker()
int_marker.header.frame_id = 'path'
int_marker.pose.position = Point(x, y, 0)
int_marker.scale = 0.1
int_marker.name = 'waypoint_' + str(id)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(copy.deepcopy(control))
control.markers.append( make_sphere(int_marker) )
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def start(self):
obj_marker = Marker()
obj_marker.type = Marker.CUBE
obj_marker.pose.orientation.w = 1
obj_marker.scale.x = 0.05
obj_marker.scale.y = 0.05
obj_marker.scale.z = 0.05
obj_marker.color.r = 1
obj_marker.color.g = 1
obj_marker.color.a = 0.7
obj_control = InteractiveMarkerControl()
obj_control.interaction_mode = InteractiveMarkerControl.MENU
obj_control.always_visible = True
obj_control.markers.append(obj_marker)
menu_entry1 = MenuEntry()
menu_entry1.id = 1
menu_entry1.title = 'Pick from front'
menu_entry1.command_type = MenuEntry.FEEDBACK
menu_entry2 = MenuEntry()
menu_entry2.id = 2
menu_entry2.title = 'Open gripper'
menu_entry2.command_type = MenuEntry.FEEDBACK
obj_im = InteractiveMarker()
obj_im.name = 'object'
obj_im.header.frame_id = 'base_link'
obj_im.pose.position.x = 0.7
obj_im.pose.position.z = 0.5
obj_im.pose.orientation.w = 1
obj_im.scale = 0.25
obj_im.menu_entries.append(menu_entry1)
obj_im.menu_entries.append(menu_entry2)
obj_im.controls.append(obj_control)
obj_im.controls.extend(make_6dof_controls())
self._im_server.insert(obj_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
self._update_grippers()
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def makeStaticMarker(self, position):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = "movement_controller"
int_marker.description = "Click to make Kuri move"
box_marker = self.makeBox(int_marker)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.markers.append(box_marker)
int_marker.controls.append(copy.deepcopy(control))
self._server.insert(int_marker, self.handleInput)
self._server.applyChanges()
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 _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)
def add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'):
marker = InteractiveMarker()
marker.header.frame_id = frame_id
marker.pose.position = init_position
marker.scale = 0.3
marker.name = 'camera_marker'
marker.description = 'Camera 6-DOF pose control'
# X axis rotation
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
marker.controls.append(control)
# X axis traslation
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
marker.controls.append(control)
# Y axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Y axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Z axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Z axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Add marker to server
self.server.insert(marker, self.marker_feedback)
self.server.applyChanges()
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)
def make6DofMarker(frame_id, fixed=False, description="Simple 6-DOF Control"):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.scale = 1
int_marker.name = "simple_6dof"
int_marker.description = description
# insert a box
makeBoxControl(int_marker)
if fixed:
int_marker.name += "_fixed"
int_marker.description += "\n(fixed orientation)"
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return 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
def setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = name
int_marker.description = name
int_marker.scale = 3
marker_control = InteractiveMarkerControl()
marker_control.always_visible = True
marker_control.markers.append(self.marker)
int_marker.controls.append(marker_control)
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if not translation :
#int_marker.pose.position = Point(0,0,0)
return int_marker
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
