def main():
rospy.init_node('interactive_marker_node')
server = InteractiveMarkerServer("simple_marker")
# create an interative marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "my_marker"
int_marker.description = "Simple Click Control"
int_marker.pose.position.x = 1
int_marker.pose.orientation.w = 1
# create a cube for the interactive marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
# create a control for the interactive marker
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
int_marker.controls.append(button_control)
server.insert(int_marker, handle_viz_input)
server.applyChanges()
rospy.spin()
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 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 __init__(self, frame_id, name, description, is_manager=False, speed=0.2):
InteractiveMarker.__init__(self)
self.header.frame_id = frame_id
self.name = name
self.description = description
self.speed = speed
self.marker = Marker()
self.marker.type = Marker.CYLINDER
self.marker.scale.x = 0.1
self.marker.scale.y = 0.1
self.marker.scale.z = 0.4
self.marker.pose.position.z = 0.20
if is_manager:
self.marker.color.r = 0.8
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 0.5
else:
self.marker.color.r = 0.0
self.marker.color.g = 0.8
self.marker.color.b = 0.0
self.marker.color.a = 0.5
self.marker_control = InteractiveMarkerControl()
self.marker_control.always_visible = True
self.marker_control.orientation.w = 1
self.marker_control.orientation.x = 0
self.marker_control.orientation.y = 1
self.marker_control.orientation.z = 0
self.marker_control.markers.append(self.marker)
self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.controls.append(self.marker_control)
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 create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1):
int_marker = InteractiveMarker()
int_marker.header.frame_id = path_msg.header.frame_id
int_marker.name = str(path_id)
int_marker.description = "Path {0}".format(path_id)
# line_marker = Marker()
# line_marker.type = Marker.LINE_STRIP
# line_marker.scale.x = width
# line_marker.color = color_msg
# line_marker.points = [p.pose.position for p in path_msg.poses]
# for point in line_marker.points:
# point.z += delta_z
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
# control.markers.append(line_marker)
points = [node(pose, delta_z) for pose in path_msg.poses]
for p1, p2 in zip(points[:-1], points[1:]):
control.markers.append(cylinder_between(p1, p2, color_msg, width))
for p in points:
control.markers.append(sphere_at(p, color_msg, width))
int_marker.controls.append(copy.deepcopy(control))
menu_handler = MenuHandler()
# put all the information in the main menu
#d = menu_handler.insert("Description")
for line in description:
menu_handler.insert(line)#, parent=d)
return menu_handler, 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 __init__(self, frame_id, name, description, is_manager=False):
InteractiveMarker.__init__(self)
self.header.frame_id = frame_id
self.name = name
self.description = description
self.marker = Marker()
self.marker.type = Marker.CYLINDER
self.marker.scale.x = 0.1
self.marker.scale.y = 0.1
self.marker.scale.z = 0.4
self.marker.pose.position.z = 0.20
if is_manager:
self.marker.color.r = 0.8
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 0.5
else:
self.marker.color.r = 0.0
self.marker.color.g = 0.8
self.marker.color.b = 0.0
self.marker.color.a = 0.5
self.marker_control = InteractiveMarkerControl()
self.marker_control.always_visible = True
self.marker_control.orientation.w = 1
self.marker_control.orientation.x = 0
self.marker_control.orientation.y = 1
self.marker_control.orientation.z = 0
self.marker_control.markers.append(self.marker)
self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.controls.append(self.marker_control)
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 _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 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 makeInteractiveMarker(name, description):
global fixed_frame
interactive_marker = InteractiveMarker()
interactive_marker.header.frame_id = fixed_frame
interactive_marker.name = name
interactive_marker.description = description
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?
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 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 _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(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = self._name
int_marker.description = self._name
int_marker.pose.position.x = self._x
int_marker.pose.position.y = self._y
int_marker.pose.orientation.w = 1
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
int_marker.controls.append(button_control)
self._server.insert(int_marker, self.handle_viz_input)
self._server.applyChanges()
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 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 create_int_marker():
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "my_marker"
int_marker.description = "Simple Click Control"
int_marker.pose.position.x = 1
int_marker.pose.orientation.w = 1
return int_marker
def getMarkersFromPose(poseStamped):
int_marker = InteractiveMarker()
int_marker.header.frame_id = poseStamped.header.frame_id
int_marker.pose = poseStamped.pose
int_control = InteractiveMarkerControl()
int_control.markers = createMarker()
int_marker.controls.append(int_control)
return int_marker
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 __init__(self,
frame_id,
name,
description,
is_manager=False,
speed=0.2):
InteractiveMarker.__init__(self)
marker_scale_x = rospy.get_param('~marker_scale_x', 1.0)
marker_scale_y = rospy.get_param('~marker_scale_y', 0.2)
marker_scale_z = rospy.get_param('~marker_scale_z', 0.2)
print "Marker scales " + str(marker_scale_x) + ", " + str(
marker_scale_y) + ", " + str(marker_scale_z)
self.header.frame_id = frame_id
self.name = name
self.description = description
self.speed = speed
self.marker = Marker()
self.marker.type = Marker.ARROW
self.marker.scale.x = marker_scale_x
self.marker.scale.y = marker_scale_y
self.marker.scale.z = marker_scale_z
#self.marker.pose.position.z = 0.20
if is_manager:
self.marker.color.r = 0.8
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 0.5
else:
self.marker.color.r = 0.0
self.marker.color.g = 0.8
self.marker.color.b = 0.0
self.marker.color.a = 0.5
self.marker_control = InteractiveMarkerControl()
self.marker_control.always_visible = True
self.marker_control.orientation.w = 1
self.marker_control.orientation.x = 0
self.marker_control.orientation.y = 1
self.marker_control.orientation.z = 0
self.marker_control.name = "move_plane"
self.marker_control.markers.append(self.marker)
self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.controls.append(self.marker_control)
self.marker_control2 = InteractiveMarkerControl()
self.marker_control2.orientation.w = 1
self.marker_control2.orientation.x = 0
self.marker_control2.orientation.y = 1
self.marker_control2.orientation.z = 0
self.marker_control2.name = "rotate_z"
self.marker_control2.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.controls.append(self.marker_control2)
def __init__(self, marker_type="mesh"):
"""[InteractiveMarker for Waypoints. Moves in SE2, can be a sphere or a flag. Has a red arrow]
Args:
marker_type (str, optional): [marker type "mesh" or "sphere"]. Defaults to "mesh".
"""
InteractiveMarker.__init__(self)
self.scale = params["control_scale"].data
self._setup_markers()
self._setup_controls()
def initialize_room_markers(self):
for room_id in range(0, len(self.regions)):
pose = Pose()
pose.position.x = self.centers[room_id, 0]
pose.position.y = self.centers[room_id, 1]
pose.position.z = 0.2
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "room_marker_" + str(room_id)
marker.description = "Room " + str(room_id)
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.35
box_marker.scale.y = 0.35
box_marker.scale.z = 0.35
box_marker.color.r = 0.
box_marker.color.g = 0.
box_marker.color.b = 1.
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
#marker.controls.append(box_control)
# move x
#control = InteractiveMarkerControl()
#control.orientation.w = 1
#control.orientation.x = 0
#control.orientation.y = 1
#control.orientation.z = 0
#control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.marker_server.insert(marker, self.room_feedback)
self.marker_server.applyChanges()
self.marker_server.setPose( marker.name, pose )
self.marker_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 = 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 make(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.name = self._name
int_marker.description = self._name
int_marker.pose.position.x = self._x
int_marker.pose.position.y = self._y
int_marker.pose.orientation.w = 1
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.45
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1
text_marker.pose.position.z = 1.5
text_marker.scale.x = 0.2
text_marker.scale.y = 0.2
text_marker.scale.z = 0.2
text_marker.color.r = 0.5
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1
text_marker.text = self._name
arrow_control = InteractiveMarkerControl()
arrow_control.orientation.w = 1
arrow_control.orientation.y = 1
arrow_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
arrow_control.markers.append(arrow_marker)
arrow_control.markers.append(text_marker)
arrow_control.always_visible = True
int_marker.controls.append(arrow_control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.y = 1
control.orientation.x = 0.02
control.orientation.z = 0.02
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.always_visible = True
int_marker.controls.append(control)
self._server.insert(int_marker, self.handle_viz_input)
self._server.applyChanges()
def _make_marker(self, name, pose):
"""Creates a new interactive marker.
Args:
name: The name of the marker.
pose: The geometry_msgs/Pose of the marker.
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = 'map'
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.45
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1
text_marker.pose.position.z = 1.5
text_marker.scale.x = 0.2
text_marker.scale.y = 0.2
text_marker.scale.z = 0.2
text_marker.color.r = 0.5
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1
text_marker.text = name
arrow_control = InteractiveMarkerControl()
arrow_control.orientation.w = 1
arrow_control.orientation.y = 1
arrow_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
arrow_control.markers.append(arrow_marker)
arrow_control.markers.append(text_marker)
arrow_control.always_visible = True
int_marker.controls.append(arrow_control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
return int_marker
def __init__(self,
frame_id,
name,
description,
is_manager=False,
speed=0.2,
mission_point=MissionPoint(),
point_description=''):
InteractiveMarker.__init__(self)
self.header.frame_id = frame_id
self.name = name
self.description = description
self.point_description = point_description
self.speed = speed
self.marker = Marker()
self.is_manager = is_manager
if is_manager:
self.marker.type = Marker.CUBE
self.marker.scale.x = 0.15
self.marker.scale.y = 0.15
self.marker.scale.z = 0.6
self.marker.pose.position.z = 0.3
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 0.7
else:
self.marker.type = Marker.CYLINDER
self.marker.scale.x = 0.1
self.marker.scale.y = 0.1
self.marker.scale.z = 0.4
self.marker.pose.position.z = 0.20
self.marker.color.r = 0.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
self.marker.color.a = 0.7
self.marker_control = InteractiveMarkerControl()
self.marker_control.always_visible = True
self.marker_control.orientation.w = 1
self.marker_control.orientation.x = 0
self.marker_control.orientation.y = 1
self.marker_control.orientation.z = 0
self.marker_control.markers.append(self.marker)
self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.mission_point = mission_point
self.controls.append(self.marker_control)
def get_2_dof_interactive_marker(marker_name, frame, x=0.0, y=0.0):
"""Return an interactive marker with 2 degree of freedom (X and Y axis)
in `frame` at (`x`, `y`, 0.0) position named `name`
:marker_name: string
:frame: string
:x: int
:y: int
:returns: visualization_msgs.InteractiveMarker
"""
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = marker_name
int_marker.pose.position.x = x
int_marker.pose.position.y = y
# int_marker.description = "Simple 2-DOF Control"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.SPHERE
box_marker.scale.x = box_marker.scale.y = box_marker.scale.z = 0.1
box_marker.color.r = box_marker.color.a = 1.0
box_marker.color.g = box_marker.color.b = 0.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
# add the control to the interactive marker
int_marker.controls.append(box_control)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_x"
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# add the control to the interactive marker
int_marker.controls.append(rotate_control)
rotate_control2 = InteractiveMarkerControl()
rotate_control2.orientation.z = rotate_control2.orientation.w = 0.707
rotate_control2.name = "move_y"
rotate_control2.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# add the control to the interactive marker
int_marker.controls.append(rotate_control2)
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 add_marker(self, name: str, pose: Pose2D) -> InteractiveMarker:
marker = InteractiveMarker(name=name,
pose=Pose(
position=Point(x=pose.x, y=pose.y,
z=0.),
orientation=quaternion_from_euler(
pose.theta, 0., 0.),
))
marker.header.frame_id = 'map'
sc = 0.2
z = 0.1
name_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
scale=Vector3(x=sc, y=sc, z=sc),
color=ColorRGBA(r=1., g=1., b=1., a=1.),
text=name,
)
name_marker.pose.position.x = sc * -0.1
name_marker.pose.position.z = z + sc * -0.1
marker.controls = [
InteractiveMarkerControl(
name='name',
orientation_mode=InteractiveMarkerControl.VIEW_FACING,
interaction_mode=InteractiveMarkerControl.NONE,
independent_marker_orientation=False,
always_visible=True,
markers=[name_marker],
),
InteractiveMarkerControl(
name='xaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
orientation=Quaternion(x=0., y=0., z=0.7068252, w=0.7068252),
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='yaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='turn',
interaction_mode=InteractiveMarkerControl.ROTATE_AXIS,
orientation=Quaternion(x=0., y=0.7068252, z=0., w=0.7068252),
)
]
self.marker_server.insert(marker,
feedback_callback=self.marker_feedback)
self.marker_server.applyChanges()
return marker
def _make_markers(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = self._frame_id
int_marker.name = 'golden_snitch'
int_marker.description = 'tool tip target'
# Create a sphere to mark where the tool tip should go
snitch = Marker()
snitch.type = Marker.SPHERE
snitch.scale.x = 0.1
snitch.scale.y = 0.1
snitch.scale.z = 0.1
snitch.color.r = 0.8
snitch.color.g = 0.8
snitch.color.b = 0.0
snitch.color.a = 0.7
snitch_control = InteractiveMarkerControl()
snitch_control.always_visible = True
snitch_control.markers.append(snitch)
int_marker.controls.append(snitch_control)
x_snitch_control = InteractiveMarkerControl()
x_snitch_control.name = 'move_x'
x_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
y_snitch_control = InteractiveMarkerControl()
y_snitch_control.name = 'move_y'
y_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
y_snitch_control.orientation.w = 0.7071068
y_snitch_control.orientation.x = 0.0
y_snitch_control.orientation.y = 0.0
y_snitch_control.orientation.z = 0.7071068
z_snitch_control = InteractiveMarkerControl()
z_snitch_control.name = 'move_z'
z_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
z_snitch_control.orientation.w = 0.7071068
z_snitch_control.orientation.x = 0.0
z_snitch_control.orientation.y = 0.7071068
z_snitch_control.orientation.z = 0.0
int_marker.controls.append(x_snitch_control)
int_marker.controls.append(y_snitch_control)
int_marker.controls.append(z_snitch_control)
return int_marker
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 create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def talker():
pub = rospy.Publisher("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(1)
int_marker = InteractiveMarker()
int_marker.name = "EE:goal_link_t"
controller = InteractiveMarkerControl()
controller.name = '_u1'
int_marker.controls = controller
p = InteractiveMarkerPose()
updater = InteractiveMarkerUpdate()
while not rospy.is_shutdown():
p.pose.position.x += 0.5
updater.markers = int_marker
updater.poses = p
print updater
pub.publish(updater)
r.sleep()
def create_interactive_marker(holder, id, pos, size, color, func):
# Make interactive marker for mouse selection
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base"
int_marker.name = "object"+str(id)
int_marker.pose.position.x = pos[0]
int_marker.pose.position.y = pos[1]
int_marker.pose.position.z = pos[2]
#color = [1, 0, 0]
# Add click control
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
create_shape(box_control, "object", id, pos, size=size, color=color)
# add the control to the interactive marker
int_marker.controls.append( box_control )
holder.insert(int_marker, lambda x: func(x, color))
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 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
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)
import roslib; roslib.load_manifest("interactive_markers")
from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl, Marker
def processFeedback(feedback):
p = feedback.pose.position
print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
if __name__=="__main__":
rospy.init_node("simple_marker")
# create an interactive marker server on the topic namespace simple_marker
server = InteractiveMarkerServer("simple_marker")
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.name = "my_marker"
int_marker.description = "Simple 1-DOF Control"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
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_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
rospy.init_node("interactive_simobject")
server = InteractiveMarkerServer("interactive_simobject")
rospy.loginfo("InteractiveMarkerServer created")
W = client.SimWorld()
rospy.loginfo("SimWorld connected")
object_id = sys.argv[1]
object_type = sys.argv[2]
x, y, z = float(sys.argv[3]), float(sys.argv[4]), float(sys.argv[5])
sim_object = W.add_object(object_id, object_type, x, y, z)
# create an interactive marker for our server
interactive_marker = InteractiveMarker()
interactive_marker.header.frame_id = "/map"
interactive_marker.name = object_id
interactive_marker.description = "Control" + object_id
# Create sphere as 'center' of the control
box_marker = Marker()
box_marker.type = Marker.SPHERE
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 0.1
box_marker.color.r = 1.0
box_marker.color.g = 0.2
box_marker.color.b = 0.2
box_marker.color.a = 1.0
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 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 publish_gripper(server, pose_stamped, name):
"""Publishes a marker representing a gripper.
Code taken from action_step_marker.py in PR2/PbD.
Args:
server: An InteractiveMarkerServer
pose_stamped: A PoseStamped giving the wrist_roll_link pose.
name: string, a unique name for this gripper.
"""
# Set angle of meshes based on gripper open vs closed.
angle = 28 * math.pi / 180.0 # Fully open.
STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/'
STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae'
STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae'
STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae'
# 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, 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 = Marker()
mesh1.header.frame_id = pose_stamped.header.frame_id
mesh1.mesh_use_embedded_materials = True
mesh1.type = Marker.MESH_RESOURCE
mesh1.scale.x = 1.0
mesh1.scale.y = 1.0
mesh1.scale.z = 1.0
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose = pose_stamped.pose
# Create mesh 2 (finger).
mesh2 = Marker()
mesh2.mesh_use_embedded_materials = True
mesh2.type = Marker.MESH_RESOURCE
mesh2.scale.x = 1.0
mesh2.scale.y = 1.0
mesh2.scale.z = 1.0
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = _get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = Marker()
mesh3.mesh_use_embedded_materials = True
mesh3.type = Marker.MESH_RESOURCE
mesh3.scale.x = 1.0
mesh3.scale.y = 1.0
mesh3.scale.z = 1.0
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = _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(math.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691, -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 = Marker()
mesh4.mesh_use_embedded_materials = True
mesh4.type = Marker.MESH_RESOURCE
mesh4.scale.x = 1.0
mesh4.scale.y = 1.0
mesh4.scale.z = 1.0
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = _get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = Marker()
mesh5.mesh_use_embedded_materials = True
mesh5.type = Marker.MESH_RESOURCE
mesh5.scale.x = 1.0
mesh5.scale.y = 1.0
mesh5.scale.z = 1.0
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = _get_pose_from_transform(t_distal)
# Append all meshes we made.
control = InteractiveMarkerControl()
control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5]
control.interaction_mode = InteractiveMarkerControl.NONE
interactive_marker = InteractiveMarker()
interactive_marker.controls = [control]
interactive_marker.header.frame_id = pose_stamped.header.frame_id
interactive_marker.pose = pose_stamped.pose
interactive_marker.name = name
server.insert(interactive_marker)
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)
import rospy
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from interactive_markers.interactive_marker_server import *
from geometry_msgs.msg import Pose, PoseStamped
if __name__=="__main__":
rospy.init_node('marker_teleop')
pub = rospy.Publisher('robot_interaction_interactive_marker_topic', InteractiveMarker)
robot = MoveGroupCommander("sia5d");
server = InteractiveMarkerServer("simple_marker")
# create interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "world"
int_marker.name = "my_marker"
int_marker.description = "Teleop Control"
p = robot.get_current_pose()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
p.pose.position.x = p.pose.position.x+0.05
int_marker.pose = p.pose
pub.publish(int_marker)
print 'heh'
rate.sleep()
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()
import rospy
import roslib; roslib.load_manifest("interactive_markers")
from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl, Marker
def processFeedback(feedback):
p = feedback.pose.position
print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
if __name__=="__main__":
rospy.init_node("netft_marker")
server = InteractiveMarkerServer("simple_marker")
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.name = "my_marker"
int_marker.description = "Netft control"
sphere = Marker()
sphere.type = Marker.SPHERE
sphere.scale.x = 0.45
sphere.scale.y = 0.45
sphere.scale.z = 0.45
sphere.color.r = 0.0
sphere.color.g = 0.5
sphere.color.b = 0.5
sphere.color.a = 1.0
