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 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