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 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 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 _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 make6DOFGimbal(intMarker):
"""Creates 6DOF gimbal marker controls."""
pitch = InteractiveMarkerControlMsg()
pitch.orientation.x = 1
pitch.orientation.y = 0
pitch.orientation.z = 0
pitch.orientation.w = 1
pitch.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS
pitch.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(pitch)
yaw = InteractiveMarkerControlMsg()
yaw.orientation.x = 0
yaw.orientation.y = 1
yaw.orientation.z = 0
yaw.orientation.w = 1
yaw.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS
yaw.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(yaw)
roll = InteractiveMarkerControlMsg()
roll.orientation.x = 0
roll.orientation.y = 0
roll.orientation.z = 1
roll.orientation.w = 1
roll.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS
roll.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(roll)
transX = InteractiveMarkerControlMsg()
transX.orientation.x = 1
transX.orientation.y = 0
transX.orientation.z = 0
transX.orientation.w = 1
transX.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS
transX.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(transX)
transY = InteractiveMarkerControlMsg()
transY.orientation.x = 0
transY.orientation.y = 1
transY.orientation.z = 0
transY.orientation.w = 1
transY.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS
transY.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(transY)
transZ = InteractiveMarkerControlMsg()
transZ.orientation.x = 0
transZ.orientation.y = 0
transZ.orientation.z = 1
transZ.orientation.w = 1
transZ.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS
transZ.orientation_mode = InteractiveMarkerControlMsg.FIXED
intMarker.controls.append(transZ)
def _create_marker(self, name):
self.name_markers[name] = InteractiveMarker()
self.name_markers[name].header.frame_id = "map"
self.name_markers[name].name = name
self.name_markers[name].description = name
self.name_markers[
name].pose.position.x = self.pose.pose.pose.position.x
self.name_markers[
name].pose.position.y = self.pose.pose.pose.position.y
# TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL
self.name_markers[name].pose.position.z = 1
box_marker = create_box_marker()
self.name_controls[name] = InteractiveMarkerControl()
self.name_controls[
name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.name_controls[name].always_visible = True
self.name_controls[name].orientation.w = 1
self.name_controls[name].orientation.x = 0
self.name_controls[name].orientation.y = 1
self.name_controls[name].orientation.z = 0
new_controller_lol = InteractiveMarkerControl()
new_controller_lol.orientation.w = 1
new_controller_lol.orientation.x = 0
new_controller_lol.orientation.y = 1
new_controller_lol.orientation.z = 0
new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED
self.name_markers[name].controls.append(new_controller_lol)
# self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w
# self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x
# self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y
# self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z
self.name_controls[name].markers.append(box_marker)
self.name_markers[name].controls.append(self.name_controls[name])
self.server.insert(self.name_markers[name], self._callback)
self.server.applyChanges()
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 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 = 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 makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def 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 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 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 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 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 talker():
rospy.init_node('talker', anonymous=True)
# desired_pose_publisher = rospy.Publisher('/equilibrium_pose', Pose, queue_size=10)
interactive_marker_init_publisher = rospy.Publisher(
'/equilibrium_pose_marker/update_full',
InteractiveMarkerInit,
queue_size=10)
rate = rospy.Rate(30) # Hz
seq_num = 1693
# interactive_marker.type = 1 # MAY NEED TO UNCOMMENT IF IT DOESNT WORK
# Topic to be published
interactive_marker_init = InteractiveMarkerInit()
interactive_marker_init.server_id = "/interactive_marker"
interactive_marker = InteractiveMarker()
interactive_marker.name = "equilibrium_pose"
interactive_marker.header.frame_id = "panda_link0"
interactive_marker.scale = 0.3
interactive_marker.description = "Equilibrium Pose\nBE CAREFUL! If you move the \nequilibrium pose the robot will follow\
\ it\nso be aware of potential collisions"
# Controls - X direction
imc = InteractiveMarkerControl()
imc.name = "rotate_x"
imc.orientation.x = 1.0
imc.orientation.y = 0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_x"
imc.orientation.x = 1.0
imc.orientation.y = 0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
# Controls - Y direction
imc = InteractiveMarkerControl()
imc.name = "rotate_y"
imc.orientation.x = 0
imc.orientation.y = 1.0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_y"
imc.orientation.x = 0
imc.orientation.y = 1.0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
# Controls - Z direction
imc = InteractiveMarkerControl()
imc.name = "rotate_z"
imc.orientation.x = 0
imc.orientation.y = 0
imc.orientation.z = 1.0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_z"
imc.orientation.x = 0
imc.orientation.y = 0
imc.orientation.z = 1.0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
desired_pose = Pose()
desired_pose.position.x = 0.441047421421
desired_pose.position.y = 0.00039597222295
desired_pose.position.z = 0.428872718226
desired_pose.orientation.x = 0.998920857906
desired_pose.orientation.y = -0.0855322959381
desired_pose.orientation.z = 0.0364367915829
desired_pose.orientation.w = -0.00293869199231
interactive_marker.pose = desired_pose
interactive_marker_init.markers.append(InteractiveMarker())
positive_y_thresh = 0.20
negative_y_thresh = -0.10
rospy.loginfo("Publishing!!")
while desired_pose.position.y < positive_y_thresh:
desired_pose.position.y += 0.001
rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)",
desired_pose.position.x, desired_pose.position.y,
desired_pose.position.z, desired_pose.orientation.x,
desired_pose.orientation.y, desired_pose.orientation.z,
desired_pose.orientation.w)
interactive_marker_init.seq_num = seq_num
interactive_marker.pose = desired_pose
interactive_marker_init.markers[0] = interactive_marker
interactive_marker_init_publisher.publish(interactive_marker_init)
seq_num = seq_num + 1
rate.sleep()
while desired_pose.position.y > negative_y_thresh:
desired_pose.position.y -= 0.001
rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)",
desired_pose.position.x, desired_pose.position.y,
desired_pose.position.z, desired_pose.orientation.x,
desired_pose.orientation.y, desired_pose.orientation.z,
desired_pose.orientation.w)
interactive_marker_init.seq_num = seq_num
interactive_marker.pose = desired_pose
interactive_marker_init.markers[0] = interactive_marker
interactive_marker_init_publisher.publish(interactive_marker_init)
seq_num = seq_num + 1
rate.sleep()
def make6DofMarker(fixed, interaction_mode, position, show_6dof=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link_zero"
int_marker.pose.position = position
#orientation with Z down (as the gripper)
int_marker.pose.orientation.x = 0.707106
int_marker.pose.orientation.y = 0.707106
int_marker.pose.orientation.z = 0
int_marker.pose.orientation.w = 0
int_marker.scale = 0.05
int_marker.name = "simple_3dof"
int_marker.description = "Goal for the VSA arm"
# 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 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 createInteractiveMarkerRGBD(self, x=0, y=0, z=0):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.parent
self.marker.pose.position.x = x
self.marker.pose.position.y = y
self.marker.pose.position.z = z
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_labeler'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 1
marker_box.color.g = 0
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_3D
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
self.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_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.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_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
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 createInteractiveMarker(world_link):
marker = InteractiveMarker()
marker.header.frame_id = world_link
trans = (1, 0, 1)
marker.pose.position.x = trans[0]
marker.pose.position.y = trans[1]
marker.pose.position.z = trans[2]
quat = (0, 0, 0, 1)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale = 0.2
marker.name = 'menu'
marker.description = 'menu'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = marker.scale * 0.7
marker_box.scale.y = marker.scale * 0.7
marker_box.scale.z = marker.scale * 0.7
marker_box.color.r = 0
marker_box.color.g = 1
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
marker.controls.append(control)
marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_3D
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
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
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
marker.controls.append(control)
server.insert(marker, markerFeedback)
menu_handler.apply(server, marker.name)
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 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)
