def init_int_marker(self):
self.ims = InteractiveMarkerServer("simple_marker")
self.im = InteractiveMarker()
self.im.header.frame_id = "/ned"
self.im.name = "my_marker"
self.im.description = "Simple 1-DOF control"
bm = Marker()
bm.type = Marker.CUBE
bm.scale.x = bm.scale.y = bm.scale.z = 0.45
bm.color.r = 0.0
bm.color.g = 0.5
bm.color.b = 0.5
bm.color.a = 1.0
bc = InteractiveMarkerControl()
bc.always_visible = True
bc.markers.append(bm)
self.im.controls.append(bc)
rc = InteractiveMarkerControl()
rc.name = "move_x"
rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.im.controls.append(rc)
self.ims.insert(self.im, self.process_marker_feedback)
self.ims.applyChanges()
def makeInteractiveMarkerControl(interactive_marker, mode):
interactive_marker_control = InteractiveMarkerControl()
interactive_marker_control.always_visible = True
interactive_marker_control.interaction_mode = mode
interactive_marker.controls.append(interactive_marker_control)
return interactive_marker_control
def make_6DOF_marker(int_marker):
"""
Creates a 6DOF InteractiveMarkerControl that can be translated and rotated.
Parameters:
int_marker: a previously created InteractiveMarker to attach the new marker
to.
"""
#x movement
control = InteractiveMarkerControl()
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
int_marker.controls.append(control);
#x rotation
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
int_marker.controls.append(control);
#y movement
control = InteractiveMarkerControl()
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
int_marker.controls.append(control);
#y rotation
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
int_marker.controls.append(control);
#z movement
control = InteractiveMarkerControl()
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
int_marker.controls.append(control);
#z rotation
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
int_marker.controls.append(control);
def makeInteractiveMarkerControl(interactive_marker, mode):
interactive_marker_control = InteractiveMarkerControl()
interactive_marker_control.always_visible = True
interactive_marker_control.interaction_mode = mode
interactive_marker.controls.append(interactive_marker_control)
return interactive_marker_control
def makeGripperMarker(angle=0.541, color=None, scale=1.0):
"""
Creates an InteractiveMarkerControl with the PR2 gripper shape.
Parameters:
angle: the aperture angle of the gripper (default=0.541)
color: (r,g,b,a) tuple or None (default) if using the material colors
scale: the scale of the gripper, default is 1.0
Returns:
The new gripper InteractiveMarkerControl
"""
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, 0.01, 0.)
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
control = InteractiveMarkerControl()
mesh = Marker()
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = scale
mesh.scale.y = scale
mesh.scale.z = scale
if color is not None:
mesh.color.r = color[0]
mesh.color.g = color[1]
mesh.color.b = color[2]
mesh.color.a = color[3]
mesh.mesh_use_embedded_materials = False
else:
mesh.mesh_use_embedded_materials = True
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.pose.orientation.w = 1
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, -0.01, 0.)
T1.rotate_axis(numpy.pi, euclid.Vector3(1,0,0))
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
