def initialize_object_marker(self, object_id, pose):
print "Adding interactive marker for object: ", object_id
color = self.object_id_color(object_id)
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "object_marker_" + str(object_id)
marker.description = "Object " + str(object_id)
click_marker = InteractiveMarker()
click_marker.header.frame_id = "map"
click_marker.name = "button_object_marker_" + str(object_id)
click_marker.description = ""
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.25
box_marker.scale.y = 0.25
box_marker.scale.z = 0.25
box_marker.color.r = color[0]
box_marker.color.g = color[1]
box_marker.color.b = color[2]
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)
# 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
control.name = "move_plane"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
marker.controls.append(control)
self.marker_poses[object_id] = pose
self.previous_poses[object_id] = pose
self.marker_server.insert(marker, self.marker_feedback)
self.marker_server.applyChanges()
pose.position.z = 0.15
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
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 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 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_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 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 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 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 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 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 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 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 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 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 ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(current_marker).always_visible
visual.interaction_mode = GetVisualControl(current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']:
new_marker.controls.append(CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def _make_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 _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 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 _marker_style(self, name, pose):
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.5
arrow_marker.scale.y = 0.07
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.5
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 get_3_dof_interactive_marker(marker_name, frame, x=0.0, y=0.0, theta=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
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, theta)
int_marker.pose.orientation = Quaternion(*quat)
int_marker.description = marker_name
# create a white box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 1.0
box_marker.scale.y = 0.1
box_marker.scale.z = 1.0
box_marker.color.r = box_marker.color.a = box_marker.color.g = box_marker.color.b = 1.0
box_marker.pose.position.z = -0.5
box_marker.pose.orientation.w = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
box_control.name = "move_x_y"
box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
box_control.orientation.w = box_control.orientation.y = 1.0
int_marker.controls.append(box_control)
rotate_control = InteractiveMarkerControl()
rotate_control.name = "rotate_z"
rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rotate_control.orientation.y = rotate_control.orientation.w = 1.0
int_marker.controls.append(rotate_control);
return int_marker
def makeAfterImage(self, data):
if theBoo == True:
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "my_marker"
int_marker.description = "After image"
position = Point(data.x, data.y, data.z)
#self.markers_pub = rospy.Publisher("map2", Marker, queue_size=1)
marker = Marker()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.scale.x = 0.45
marker.scale.y = 0.45
marker.scale.z = 0.45
marker.pose.position = position
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.header.stamp = rospy.get_rostime()
marker.id = 1
#self.markers_pub.publish(marker)
#server.applyChanges()
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append( marker )
# add the control to the interactive marker
int_marker.controls.append( box_control )
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, self.processFeedback)
def create_marker(self, pose_name):
# Creates interactive marker with metadata, pose
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.name = pose_name
int_marker.description = "Interactive Marker for Pose"
int_marker.pose.position.x = 0
int_marker.pose.position.y = 0
int_marker.pose.position.z = 0
int_marker.pose.orientation.w = 1
# Create arrow and circle
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.scale.x = 0.5 # put it in place
arrow_marker.scale.y = 0.5
arrow_marker.scale.z = 0.5
arrow_marker.color.r = 0.0 # make it pretty
arrow_marker.color.g = 1.0
arrow_marker.color.b = 0.0
arrow_marker.color.a = 1.0
# Create a non-interactive control which controls the arrow
rotate_control = InteractiveMarkerControl()
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
rotate_control.always_visible = True
rotate_control.orientation.w = 1
rotate_control.orientation.x = 0
rotate_control.orientation.y = 1
rotate_control.orientation.z = 0
rotate_control.markers.append(arrow_marker)
# Add the above control to the interactive marker
int_marker.controls.append(rotate_control)
# Apply changes to our marker server
print 'inserting marker'
print self._marker_server
self._marker_server.insert(int_marker, self.processFeedback)
self._marker_server.applyChanges()
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 make_quadcopter_marker():
marker = InteractiveMarker()
marker.header.frame_id = 'base_link'
marker.header.stamp = rospy.get_rostime()
marker.scale = 1
marker.pose.orientation.w = 1
marker.name = 'quadcopter'
marker.description = 'Quadcopter'
make_box_control(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
marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
return marker
def __init__(self, server, name, X_WM, markers):
self._server = server
self.name = name
self._X_WM = X_WM
self._menu = MenuHandler()
self._edit_id = self._menu.insert("Edit", callback=self._edit_callback)
self._delete_id = self._menu.insert("Delete", callback=self._delete_callback)
self._menu.insert(self.name, callback=noop) # For annotation.
self._t_click_prev = -float('inf')
# Make template marker.
template = InteractiveMarker()
template.name = self.name
template.description = name
template.header.frame_id = "world"
viz = InteractiveMarkerControl()
viz.interaction_mode = InteractiveMarkerControl.BUTTON
viz.always_visible = True
viz.markers.extend(markers)
template.controls.append(viz)
self._template = template
# Initialize.
self._edit = False
self._update()
def 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 __init__(self, server):
self.marker_name = "IMU"
self.imu_publisher = self.create_publisher(Imu, "/imu/data", 1)
self.server = server
self.pose = Pose()
self.pose.orientation.w = 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "imu_frame"
int_marker.pose = self.pose
int_marker.scale = 1
int_marker.name = self.marker_name
int_marker.description = "Rotate 2DOF to simulate IMU orientation to ground"
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
normalize_quaternion(control.orientation)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.always_visible = True
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
normalize_quaternion(control.orientation)
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(int_marker, self.process_feedback)
def main():
rospy.init_node('interactive_marker_demo')
wait_for_time()
server = InteractiveMarkerServer("simple_marker")
marker = InteractiveMarker()
marker.header.frame_id = "base_link"
marker.name = "adamson_marker"
marker.description = "Simple Click Control"
marker.pose.position.x = 1
marker.pose.orientation.w = 1
print("This is my interactive marker:")
pprint(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
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
marker.controls.append(button_control)
print("\nAnd this is my normal marker")
pprint(box_marker)
server.insert(marker, handle_vix_input)
server.applyChanges()
rospy.spin()
def makeQuadrocopterMarker(self, position):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = "look_at_point"
int_marker.description = "Point to look at"
self.makeBoxControl(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.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(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)
link_name = rospy.get_param("~link_name")
# Get initial pose for the interactive marker
while not initial_pose_found:
rospy.sleep(1)
state_sub.unregister()
pose_pub = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
server = InteractiveMarkerServer("equilibrium_pose_marker")
int_marker = InteractiveMarker()
int_marker.header.frame_id = link_name
int_marker.scale = 0.3
int_marker.name = "equilibrium_pose"
int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
"If you move the \nequilibrium "
"pose the robot will follow it\n"
"so be aware of potential collisions")
int_marker.pose = marker_pose.pose
# run pose publisher
rospy.Timer(rospy.Duration(0.005),
lambda msg: publisherCallback(msg, link_name))
# insert a box
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
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 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
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
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
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
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 makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "" # "EEF 6DOF control"
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
# self.menu_handler.insert("Do stuff", callback=self.processFeedback)
# This makes the floating text appear
# make one control using default visuals
# control = InteractiveMarkerControl()
# control.interaction_mode = InteractiveMarkerControl.MENU
# control.description="Menu"
# control.name = "menu_only_control"
# int_marker.controls.append(copy.deepcopy(control))
# marker = self.makeArrow( int_marker )
# control.markers.append( marker )
# control.always_visible = False
# int_marker.controls.append(control)
self.server.insert(int_marker, self.processFeedback)
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 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
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
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()
