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 __init__(self,
root,
name,
description,
server,
ns="",
position0=[0., 0., 0.]):
# Make interactive marker
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = root
self.int_marker.pose.position = Point(*position0)
self.int_marker.scale = 0.1
self.int_marker.name = name
self.name = name
self.int_marker.description = description
# Make the way it should look
mrkrmsg = Marker()
mrkrmsg.type = Marker.CUBE
mrkrmsg.scale.x = 0.03
mrkrmsg.scale.y = 0.03
mrkrmsg.scale.z = 0.03
mrkrmsg.color.r = 0.7
mrkrmsg.color.a = 1.0
# Make the Inner cube control mode:
cubecntrl = InteractiveMarkerControl()
cubecntrl.always_visible = True
cubecntrl.interaction_mode = InteractiveMarkerControl.MOVE_3D
cubecntrl.markers.append(mrkrmsg)
controls = [cubecntrl]
# Make XYZ linear motion control modes:
for i in range(3):
direction = [0] * i + [1] + [0] * (2 - i)
control = InteractiveMarkerControl()
control.name = "move_" + chr(ord("x") + i)
control.orientation = Quaternion(*(direction + [1]))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
controls.append(control)
# Make XYZ rotation control modes:
for i in range(3):
direction = [0] * i + [1] + [0] * (2 - i)
control = InteractiveMarkerControl()
control.name = "rotate_" + chr(ord("x") + i)
control.orientation = Quaternion(*(direction + [1]))
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
controls.append(control)
# Add them to the interactive marker
self.int_marker.controls = controls
# Setup pose publisher
self.pose_pub = rospy.Publisher(ns + name, Pose, queue_size=1)
# Setup the interactive marker in the server
self.server = server
self.server.insert(self.int_marker, self.callbackPublish)
self.server.applyChanges()
def 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 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 _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 _CreateMarkerControl(name, orientation, marker_type):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.interaction_mode = marker_type
control.always_visible = False
return control
def _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 _CreateMarkerControl(name, orientation, marker_type):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.interaction_mode = marker_type
control.always_visible = False
return control
def CreateVisualControlFromMarker(marker, always_visible=True, interaction_mode=InteractiveMarkerControl.MENU):
control = InteractiveMarkerControl()
control.name = "visual"
control.always_visible = always_visible
control.interaction_mode = interaction_mode
control.markers.append(marker)
return control
def make_interactive_marker(pose, model):
int_marker = InteractiveMarker()
int_marker.name = model
int_marker.description = model
int_marker.header.frame_id = "map"
int_marker.pose = pose
int_marker.pose.position.z = max(int_marker.pose.position.z,
0.01) # ensure marker is above ground
int_marker.pose.orientation = quaternion_msg_from_yaw(
yaw(pose)) # discard all but yaw to ensure marker is flat
int_marker.scale = 1
make_model_control(int_marker, model)
control = InteractiveMarkerControl()
control.name = "drag"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalize_quaternion(control.orientation)
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, process_feedback)
menu_handler.apply(server, int_marker.name)
return int_marker
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for hte control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self.is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def 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 CreatePrimitiveControl(name, scaleFactor, marker_type, id=randint(0,10000)):
marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id)
control = InteractiveMarkerControl()
control.name = name
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
control.markers.append(marker)
return control
def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description = "Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
server.applyChanges()
def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description="Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
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 axis_control(name, q):
cr = InteractiveMarkerControl()
cr.orientation.x = q[0]
cr.orientation.y = q[1]
cr.orientation.z = q[2]
cr.orientation.w = q[3]
cr.name = name
return cr
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 add_new_marker(self,
name,
body,
callback,
selected=True,
delay_update=False,
frame=None,
pose=None):
"""Adds a new interactive marker to the node.
:param name: Name of the marker
:type name: str
:param body: Body controlled by the marker
:type body: RigidBody, MultiBody
:param callback: Callback function for the marker
:type callback: function
:param selected: Should the marker be selected upon creation
:type selected: bool
:param delay_update: Delay the update to the server
:type delay_update: bool
:param frame: Name of reference frame
:type frame: NoneType, str
:param pose: Initial pose of the marker
:type pose: NoneType, Frame
"""
intMarker = InteractiveMarker()
intMarker.name = name
intMarker.header.frame_id = frame if frame is not None else name
intMarker.header.stamp = rospy.Time(0)
intMarker.scale = 1.0
if pose is not None:
intMarker.pose.position.x = pose.position[0]
intMarker.pose.position.y = pose.position[1]
intMarker.pose.position.z = pose.position[2]
intMarker.pose.orientation.x = pose.quaternion[0]
intMarker.pose.orientation.y = pose.quaternion[1]
intMarker.pose.orientation.z = pose.quaternion[2]
intMarker.pose.orientation.w = pose.quaternion[3]
control = InteractiveMarkerControlMsg()
control.interaction_mode = InteractiveMarkerControlMsg.MOVE_3D
control.always_visible = True
control.orientation.w = 1.0
control.name = 'visual'
control.description = name
intMarker.controls.append(control)
make6DOFGimbal(intMarker)
self.create_visual_marker(name, body, intMarker, control)
activateMarker(intMarker, selected)
self.marker_server.insert(intMarker, callback)
self.markers[name] = (intMarker, callback)
body.register_deletion_cb(self.on_obj_deleted)
if not delay_update:
self.marker_server.applyChanges()
def make_rotate_axis_fixed(axis_name, orientation):
controlR = InteractiveMarkerControl()
controlR.orientation.w = orientation[0]
controlR.orientation.x = orientation[1]
controlR.orientation.y = orientation[2]
controlR.orientation.z = orientation[3]
controlR.name = "rotate_" + axis_name
controlR.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed_orientation:
controlR.orientation_mode = InteractiveMarkerControl.FIXED
controlM = InteractiveMarkerControl()
controlM.orientation.w = orientation[0]
controlM.orientation.x = orientation[1]
controlM.orientation.y = orientation[2]
controlM.orientation.z = orientation[3]
controlM.name = "move_" + axis_name
controlM.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
controlM.orientation_mode = InteractiveMarkerControl.FIXED
return (controlR, controlM)
def CreateVisualControlFromMarker(
marker,
always_visible=True,
interaction_mode=InteractiveMarkerControl.MENU):
control = InteractiveMarkerControl()
control.name = "visual"
control.always_visible = always_visible
control.interaction_mode = interaction_mode
control.markers.append(marker)
return control
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 CreatePrimitiveControl(name,
scaleFactor,
marker_type,
id=randint(0, 10000)):
marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id)
control = InteractiveMarkerControl()
control.name = name
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
control.markers.append(marker)
return control
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(current_marker).always_visible
visual.interaction_mode = GetVisualControl(current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']:
new_marker.controls.append(CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def six_dof_controls():
"""Returns a list of 6 InteractiveMarkerControls
"""
controls = []
# Create 6 DOF controls
rx_control = InteractiveMarkerControl()
rx_control.orientation.w = 1
rx_control.orientation.x = 1
rx_control.orientation.y = 0
rx_control.orientation.z = 0
rx_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rx_control.name = 'rotate_x'
controls.append(rx_control)
mx_control = InteractiveMarkerControl()
mx_control.orientation.w = 1
mx_control.orientation.x = 1
mx_control.orientation.y = 0
mx_control.orientation.z = 0
mx_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
mx_control.name = 'move_x'
controls.append(mx_control)
rz_control = InteractiveMarkerControl()
rz_control.orientation.w = 1
rz_control.orientation.x = 0
rz_control.orientation.y = 1
rz_control.orientation.z = 0
rz_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rz_control.name = 'rotate_z'
controls.append(rz_control)
mz_control = InteractiveMarkerControl()
mz_control.orientation.w = 1
mz_control.orientation.x = 0
mz_control.orientation.y = 1
mz_control.orientation.z = 0
mz_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
mz_control.name = 'move_z'
controls.append(mz_control)
ry_control = InteractiveMarkerControl()
ry_control.orientation.w = 1
ry_control.orientation.x = 0
ry_control.orientation.y = 0
ry_control.orientation.z = 1
ry_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
ry_control.name = 'rotate_y'
controls.append(ry_control)
my_control = InteractiveMarkerControl()
my_control.orientation.w = 1
my_control.orientation.x = 0
my_control.orientation.y = 0
my_control.orientation.z = 1
my_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
my_control.name = 'move_y'
controls.append(my_control)
return controls
def make_interactive_marker(name, pose, frame='base', color=(0.5, 0.5, 0.5)):
rospy.loginfo("Creating imarker...")
int_marker = interactive_marker_server.InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.header.stamp = rospy.Time.now()
int_marker.scale = 0.25
int_marker.name = name
int_marker.description = "interactive marker"
cylinder_marker = Marker()
cylinder_marker.type = Marker.CYLINDER
cylinder_marker.scale.x = 0.25
cylinder_marker.scale.y = 0.25
cylinder_marker.scale.z = 0.25
cylinder_marker.color.r = color[0]
cylinder_marker.color.g = color[1]
cylinder_marker.color.b = color[2]
cylinder_marker.color.a = 0.5
cylinder_control = InteractiveMarkerControl()
cylinder_control.always_visible = True
cylinder_control.markers.append(cylinder_marker)
int_marker.controls.append(cylinder_control)
for axis, orientation in zip(['x', 'y', 'z'], [(1, 1, 0, 0), (1, 0, 1, 0),
(1, 0, 0, 1)]):
control = InteractiveMarkerControl()
for attr, val in zip(['w', 'x', 'y', 'z'], orientation):
setattr(control.orientation, attr, val)
control.name = "rotate_{}".format(axis)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control.name = "move_{}".format(axis)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
int_marker.pose = pose
rospy.loginfo("Imarker created")
return int_marker
def make_6dof_controls():
# Edit axes.
# N.B. Follow order from tutorial?
n = 1 / math.sqrt(2)
axes = [
('x', assign(Quaternion(), w=n, x=n, y=0, z=0)),
('z', assign(Quaternion(), w=n, x=0, y=0, z=n)),
('y', assign(Quaternion(), w=n, x=0, y=n, z=0)),
]
controls = []
for name, quat in axes:
control = InteractiveMarkerControl()
control.orientation = quat
control.name = "rotate_" + name
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
controls.append(control)
control = InteractiveMarkerControl()
control.orientation = quat
control.name = "move_" + name
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
controls.append(control)
return controls
def make_cont(self, parent):
"""Creates the controller for the marker for the side of an object.
Args:
parent (Marker)
Returns:
control (MarkerController)
"""
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
control.name = parent.ns
return control
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 __init__(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "/world"
self.int_marker.name = "my_marker"
self.int_marker.description = "Simple 2-DOF Control"
self.int_marker.scale = 0.5
self.mesh_marker = MeshMarker()
self.mesh_marker.set_color(color=(1.0, 1.0, 1.0, 0.5))
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = False
box_control.markers.append(self.mesh_marker.marker)
# add the control to the interactive marker
self.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
control = InteractiveMarkerControl()
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.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
self.int_marker.controls.append(control)
self.set_position()
self.set_orientation()
def __init__(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "/world"
self.int_marker.name = "my_marker"
self.int_marker.description = "Simple 2-DOF Control"
self.int_marker.scale = 0.5
self.mesh_marker = MeshMarker()
self.mesh_marker.set_color(color=(1.0,1.0,1.0,0.5))
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = False
box_control.markers.append(self.mesh_marker.marker)
# add the control to the interactive marker
self.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
control = InteractiveMarkerControl()
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.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
self.int_marker.controls.append(control)
self.set_position()
self.set_orientation()
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def make_6dof_controls():
"""Returns a list of 6 InteractiveMarkerControls
"""
controls = []
# Add 6 DOF controls
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.name = 'rotate_x'
controls.append(copy.deepcopy(control))
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = 'move_x'
controls.append(copy.deepcopy(control))
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.name = 'rotate_z'
controls.append(copy.deepcopy(control))
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = 'move_z'
controls.append(copy.deepcopy(control))
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.name = 'rotate_y'
controls.append(copy.deepcopy(control))
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = 'move_y'
controls.append(copy.deepcopy(control))
return controls
def add_marker(self,
init_position,
callback,
controls=[],
scale=1.,
marker=None,
name=None,
fixed_orientation=False):
if name is None:
name = 'control%d' % (self.marker_ctr)
self.marker_ctr += 1
if marker is None:
marker = make_markers.make_marker()
marker.header.frame_id = self.frame_id
marker.header.frame_id = self.frame_id
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.pose.position = init_position
int_marker.scale = scale
int_marker.name = name
vm_control = InteractiveMarkerControl()
vm_control.always_visible = True
vm_control.markers.append(marker)
int_marker.controls.append(vm_control)
#int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
for control_name in controls:
data = TYPEDATA[control_name]
control = InteractiveMarkerControl()
control.orientation.w = data[0]
control.orientation.x = data[1]
control.orientation.y = data[2]
control.orientation.z = data[3]
control.name = control_name
control.interaction_mode = data[4]
if fixed_orientation:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.server.insert(int_marker, callback)
self.markers[name] = int_marker
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
# control_marker.header.frame_id = self.robot.get_root()
control_marker.header.frame_id = "base"
control_marker.name = "cc_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
# 'commit' changes and send to all clients
self.server.applyChanges()
def __init__(self):
rospy.sleep(1.0)
self.items = [
'pinger_surface', 'pinger_shooter', 'vampire_slayer',
'start_gate1', 'start_gate2'
]
self.guess_service = rospy.Service('guess_location', GuessRequest,
self.request_location)
self.markers_subscribers = []
self.markers_locations = dict.fromkeys(self.items)
self.markers_servers = []
self.markers = []
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
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_x"
rotate_control.orientation.w = 0.707
rotate_control.orientation.x = 0
rotate_control.orientation.y = 0.707
rotate_control.orientation.z = 0
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
spacer = 0
for i in self.items:
self.markers.append(InteractiveMarker())
self.markers[spacer].header.frame_id = "/map"
self.markers[spacer].name = i
self.markers[spacer].description = i
self.markers[spacer].controls.append(box_control)
self.markers[spacer].controls.append(rotate_control)
self.markers[spacer].pose.position.x = spacer
self.markers[spacer].pose.position.y = 0
self.markers[spacer].pose.position.z = 0
spacer = spacer + 1
def __init__(self):
rospy.sleep(1.0)
self.items = [
'pinger1', 'pinger2', 'dice', 'start_gate1', 'start_gate2'
]
self.guess_service = rospy.Service('guess_location', GuessRequest,
self.request_location)
self.markers_subscribers = []
self.markers_locations = dict.fromkeys(self.items)
self.markers_servers = []
self.markers = []
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
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_x"
rotate_control.orientation.w = 0.707
rotate_control.orientation.x = 0
rotate_control.orientation.y = 0.707
rotate_control.orientation.z = 0
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
spacer = 0
for i in self.items:
self.markers.append(InteractiveMarker())
self.markers[spacer].header.frame_id = "map"
self.markers[spacer].name = i
self.markers[spacer].description = i
self.markers[spacer].controls.append(box_control)
self.markers[spacer].controls.append(rotate_control)
self.markers[spacer].pose.position.x = spacer
self.markers[spacer].pose.position.y = 0
self.markers[spacer].pose.position.z = 0
spacer = spacer + 1
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 init_marker(self):
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/world_link"
control_marker.name = "cg_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.server.applyChanges()
def make6DOFControls():
translation_x_control = InteractiveMarkerControl()
translation_x_control.name = "move_x"
translation_x_control.orientation.w = 1
translation_x_control.orientation.x = 1
translation_x_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translation_y_control = InteractiveMarkerControl()
translation_y_control.name = "move_y"
translation_y_control.orientation.w = 1
translation_y_control.orientation.y = 1
translation_y_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translation_z_control = InteractiveMarkerControl()
translation_z_control.name = "move_z"
translation_z_control.orientation.w = 1
translation_z_control.orientation.z = 1
translation_z_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
rotation_x_control = InteractiveMarkerControl()
rotation_x_control.name = "rotate_x"
rotation_x_control.orientation.w = 1
rotation_x_control.orientation.x = 1
rotation_x_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rotation_y_control = InteractiveMarkerControl()
rotation_y_control.name = "rotate_y"
rotation_y_control.orientation.w = 1
rotation_y_control.orientation.y = 1
rotation_y_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rotation_z_control = InteractiveMarkerControl()
rotation_z_control.name = "rotate_z"
rotation_z_control.orientation.w = 1
rotation_z_control.orientation.z = 1
rotation_z_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
return [translation_x_control, translation_y_control,
translation_z_control,
rotation_x_control, rotation_y_control,
rotation_z_control]
box_control.always_visible = True
box_control.markers.append(box_marker)
# add the control to the interactive marker
interactive_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
translate_x = InteractiveMarkerControl()
translate_x.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translate_x.orientation.w = 1
translate_x.orientation.x = 1
translate_x.orientation.y = 0
translate_x.orientation.z = 0
translate_x.name = "translate_x"
interactive_marker.controls.append(translate_x)
translate_y = InteractiveMarkerControl()
translate_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translate_y.orientation.w = 1
translate_y.orientation.x = 0
translate_y.orientation.y = 0
translate_y.orientation.z = 1
translate_y.name = "translate_y"
interactive_marker.controls.append(translate_y)
translate_z = InteractiveMarkerControl()
translate_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translate_z.orientation.w = 1
translate_z.orientation.x = 0
def createMoveControls(fixed=False):
controls = []
## rotate control x
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
controls.append(control)
## rotate control y
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## rotate control z
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## move control x
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
controls.append(control)
## move control y
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## move control z
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
return controls
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 init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/world_link"
control_marker.name = "cc_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Run grader", callback=self.run_grader_cb)
self.menu_handler.apply(self.server, "cc_marker",)
redundancy_marker = InteractiveMarker()
redundancy_marker.header.frame_id = "/lwr_arm_1_link"
redundancy_marker.name = "red_marker"
rotate_control = InteractiveMarkerControl()
rotate_control.name = "rotate_z"
rotate_control.orientation.w = 1
rotate_control.orientation.y = 1
rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
redundancy_marker.controls.append(rotate_control)
redundancy_marker.scale = 0.25
self.server.insert(redundancy_marker, self.redundancy_marker_feedback)
# 'commit' changes and send to all clients
self.server.applyChanges()
def __init__(self):
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
#get this from the frame_tracker parameters
if rospy.has_param('cartesian_controller/chain_tip_link'):
self.active_frame = rospy.get_param("cartesian_controller/chain_tip_link")
else:
rospy.logerr("No chain_tip_link specified. Aborting!")
sys.exit()
if rospy.has_param('cartesian_controller/tracking_frame'):
self.tracking_frame = rospy.get_param("cartesian_controller/tracking_frame")
else:
rospy.logerr("No tracking_frame specified. Aborting!")
sys.exit()
if rospy.has_param('cartesian_controller/root_frame'):
self.root_frame = rospy.get_param("cartesian_controller/root_frame")
else:
rospy.logerr("No root_frame specified. Setting to 'base_link'!")
self.root_frame = "base_link"
if rospy.has_param('cartesian_controller/movable_trans'):
self.movable_trans = rospy.get_param("cartesian_controller/movable_trans")
else:
rospy.logerr("No movable_trans specified. Setting True!")
self.movable_trans = True
if rospy.has_param('cartesian_controller/movable_rot'):
self.movable_rot = rospy.get_param("cartesian_controller/movable_rot")
else:
rospy.logerr("No movable_rot specified. Setting True!")
self.movable_rot = True
self.tracking = False
print "Waiting for StartTrackingServer..."
rospy.wait_for_service('frame_tracker/start_tracking')
print "...done!"
self.start_tracking_client = rospy.ServiceProxy('frame_tracker/start_tracking', SetString)
print "Waiting for StopTrackingServer..."
rospy.wait_for_service('frame_tracker/stop_tracking')
print "...done!"
self.stop_tracking_client = rospy.ServiceProxy('frame_tracker/stop_tracking', Empty)
self.target_pose = PoseStamped()
self.target_pose.header.stamp = rospy.Time.now()
self.target_pose.header.frame_id = self.root_frame
self.target_pose.pose.orientation.w = 1.0
##give tf_listener some time to fill cache
#try:
#rospy.sleep(1.0)
#except rospy.ROSInterruptException as e:
##print "ROSInterruptException"
#pass
transform_available = False
while not transform_available:
try:
(trans,rot) = self.listener.lookupTransform(self.root_frame, self.active_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
#rospy.logwarn("Waiting for transform...")
try:
rospy.sleep(0.1)
except rospy.ROSInterruptException as e:
#print "ROSInterruptException"
pass
continue
transform_available = True
self.target_pose.pose.position.x = trans[0]
self.target_pose.pose.position.y = trans[1]
self.target_pose.pose.position.z = trans[2]
self.target_pose.pose.orientation.x = rot[0]
self.target_pose.pose.orientation.y = rot[1]
self.target_pose.pose.orientation.z = rot[2]
self.target_pose.pose.orientation.w = rot[3]
self.ia_server = InteractiveMarkerServer("marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = self.root_frame
self.int_marker.pose = self.target_pose.pose
self.int_marker.name = "interactive_target"
self.int_marker.description = self.tracking_frame
self.int_marker.scale = 0.8
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 0.1
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
control_3d = InteractiveMarkerControl()
control_3d.always_visible = True
control_3d.name = "move_rotate_3D"
control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control_3d.markers.append( box_marker )
self.int_marker.controls.append(control_3d)
control = InteractiveMarkerControl()
control.always_visible = True
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
if(self.movable_trans):
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
if(self.movable_rot):
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
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
self.ia_server.insert(self.int_marker, self.marker_fb)
#create menu
self.menu_handler = MenuHandler()
self.menu_handler.insert( "StartTracking", callback=self.start_tracking )
self.menu_handler.insert( "StopTracking", callback=self.stop_tracking )
self.menu_handler.insert( "ResetTracking", callback=self.reset_tracking )
self.int_marker_menu = InteractiveMarker()
self.int_marker_menu.header.frame_id = self.root_frame
self.int_marker_menu.name = "marker_menu"
self.int_marker_menu.description = rospy.get_namespace()
self.int_marker_menu.scale = 1.0
self.int_marker_menu.pose.position.z = 1.2
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.name = "menu_control"
control.description= "InteractiveTargetMenu"
self.int_marker_menu.controls.append(copy.deepcopy(control))
self.ia_server.insert(self.int_marker_menu, self.menu_fb)
self.menu_handler.apply( self.ia_server, self.int_marker_menu.name )
self.ia_server.applyChanges()
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 make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
global parentFrame, frame_name
int_marker = InteractiveMarker()
int_marker.header.frame_id = parentFrame
int_marker.pose.position = position
int_marker.scale = 0.5
int_marker.name = frame_name
int_marker.description = "Place a frame with 6-DOF!"
# insert a box
makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if fixed:
int_marker.name += "_fixed"
int_marker.description += "\n(fixed orientation)"
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D : "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" }
int_marker.name += "_" + control_modes_dict[interaction_mode]
int_marker.description = "3D Control"
if show_6dof:
int_marker.description += " + 6-DOF controls"
int_marker.description += "\n" + control_modes_dict[interaction_mode]
if show_6dof:
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)
server.insert(int_marker, processFeedback)
menu_handler.apply( server, int_marker.name )
def __init__(self):
#self.client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction)
#print "Waiting for Server..."
##self.client.wait_for_server()
#print "...done!"
print "Waiting for StartTrackingServer..."
rospy.wait_for_service('/lookat_controller/start_tracking')
print "...done!"
self.start_tracking_client = rospy.ServiceProxy('/lookat_controller/start_tracking', SetString)
print "Waiting for StopTrackingServer..."
rospy.wait_for_service('/lookat_controller/stop_tracking')
print "...done!"
self.stop_tracking_client = rospy.ServiceProxy('/lookat_controller/stop_tracking', Empty)
self.target_pose = PoseStamped()
self.target_pose.header.stamp = rospy.Time.now()
self.target_pose.header.frame_id = "base_link"
self.target_pose.pose.orientation.w = 1.0
#self.viz_pub = rospy.Publisher('lookat_target', PoseStamped)
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
transform_available = False
while not transform_available:
try:
(trans,rot) = self.listener.lookupTransform('/base_link', '/odometry_monitor_target', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("Waiting for transform...")
rospy.sleep(0.5)
continue
transform_available = True
self.target_pose.pose.position.x = trans[0]
self.target_pose.pose.position.y = trans[1]
self.target_pose.pose.position.z = trans[2]
self.ia_server = InteractiveMarkerServer("marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "base_link"
self.int_marker.pose = self.target_pose.pose
self.int_marker.name = "lookat_target"
self.int_marker.description = "virtual lookat target"
self.int_marker.scale = 0.5
# create a grey box marker
#box_marker = Marker()
#box_marker.type = Marker.CUBE
#box_marker.scale.x = 0.1
#box_marker.scale.y = 0.1
#box_marker.scale.z = 0.1
#box_marker.color.r = 0.0
#box_marker.color.g = 0.5
#box_marker.color.b = 0.5
#box_marker.color.a = 1.0
#box_control = InteractiveMarkerControl()
#box_control.always_visible = True
#box_control.markers.append( box_marker )
#self.int_marker.controls.append(box_control)
#control = InteractiveMarkerControl()
#control.always_visible = True
#control.orientation.w = 1
#control.orientation.x = 1
#control.orientation.y = 0
#control.orientation.z = 0
#control.name = "move_3D"
#control.interaction_mode = InteractiveMarkerControl.MOVE_3D
#self.int_marker.controls.append(deepcopy(control))
#control.name = "move_x"
#control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "rotate_x"
#control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "move_y"
#control.orientation.x = 0
#control.orientation.y = 1
#control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "rotate_y"
#control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "move_z"
#control.orientation.y = 0
#control.orientation.z = 1
#control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "rotate_z"
#control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#
#self.ia_server.insert(self.int_marker, self.marker_fb)
#create menu
self.menu_handler = MenuHandler()
#self.menu_handler.insert( "Lookat", callback=self.lookat )
self.menu_handler.insert( "StartTracking", callback=self.start_tracking )
self.menu_handler.insert( "StopTracking", callback=self.stop_tracking )
self.int_marker_menu = InteractiveMarker()
self.int_marker_menu.header.frame_id = "base_link"
self.int_marker_menu.name = "lookat_menu"
self.int_marker_menu.description = "Menu"
self.int_marker_menu.scale = 1.0
self.int_marker_menu.pose.position.z = 1.2
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description="Lookat"
control.name = "menu_only_control"
self.int_marker_menu.controls.append(copy.deepcopy(control))
self.ia_server.insert(self.int_marker_menu, self.menu_fb)
self.menu_handler.apply( self.ia_server, self.int_marker_menu.name )
self.ia_server.applyChanges()
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
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);
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
rospy.spin()
def add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'):
marker = InteractiveMarker()
marker.header.frame_id = frame_id
marker.pose.position = init_position
marker.scale = 0.3
marker.name = 'camera_marker'
marker.description = 'Camera 6-DOF pose control'
# X axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# X axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Y axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Y axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Z axis rotation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
marker.controls.append(control)
# Z axis traslation
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
marker.controls.append(control)
# Add marker to server
self.server.insert(marker, self.marker_feedback)
self.server.applyChanges()
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def make6DofMarker( fixed = False ):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.scale = 0.05
int_marker.pose.orientation.x = frame_pose.orientation.x
int_marker.pose.orientation.y = frame_pose.orientation.y
int_marker.pose.orientation.z = frame_pose.orientation.z
int_marker.pose.orientation.w = frame_pose.orientation.w
int_marker.pose.position.x = frame_pose.position.x
int_marker.pose.position.y = frame_pose.position.y
int_marker.pose.position.z = frame_pose.position.z
int_marker.name = FRAME_ID
int_marker.description = "Place the writing surface"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = SURFACE_WIDTH
box_marker.scale.y = SURFACE_HEIGHT
box_marker.scale.z = 0.001
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append( writing_surface() )
# add the control to the interactive marker
int_marker.controls.append( box_control )
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)
server.insert(int_marker, 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 make6DofMarker( fixed = False ):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.scale = 0.05
int_marker.pose.position.x = -0.100988589227
int_marker.pose.position.y = 0.035845387727
int_marker.pose.position.z = 0.266128748655
int_marker.name = "paper_sheet"
int_marker.description = "Place the sheet of paper"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.21
box_marker.scale.y = 0.297
box_marker.scale.z = 0.001
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append( a4_sheet() )
# add the control to the interactive marker
int_marker.controls.append( box_control )
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)
server.insert(int_marker, processFeedback)
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/base"
control_marker.name = "move_arm_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
box = Marker()
box.type = Marker.CUBE
box.scale.x = 0.15
box.scale.y = 0.03
box.scale.z = 0.03
box.color.r = 0.5
box.color.g = 0.5
box.color.b = 0.5
box.color.a = 1.0
menu_control.markers.append(box)
box2 = deepcopy(box)
box2.scale.x = 0.03
box2.scale.z = 0.1
box2.pose.position.z=0.05
menu_control.markers.append(box2)
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Move Arm", callback=self.move_arm_cb)
obs_entry = self.menu_handler.insert("Obstacles")
self.menu_handler.insert("No Obstacle", callback=self.no_obs_cb, parent=obs_entry)
self.menu_handler.insert("Simple Obstacle", callback=self.simple_obs_cb, parent=obs_entry)
self.menu_handler.insert("Hard Obstacle", callback=self.complex_obs_cb, parent=obs_entry)
self.menu_handler.insert("Super-hard Obstacle", callback=self.super_obs_cb, parent=obs_entry)
options_entry = self.menu_handler.insert("Options")
self.plot_entry = self.menu_handler.insert("Plot trajectory", parent=options_entry,
callback = self.plot_cb)
self.menu_handler.setCheckState(self.plot_entry, MenuHandler.UNCHECKED)
self.menu_handler.apply(self.server, "move_arm_marker",)
self.server.applyChanges()
Ttrans = tf.transformations.translation_matrix((0.6,0.2,0.2))
Rtrans = tf.transformations.rotation_matrix(3.14159,(1,0,0))
self.server.setPose("move_arm_marker", convert_to_message(numpy.dot(Ttrans,Rtrans)))
self.server.applyChanges()
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 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)
