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()
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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);
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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