def _get_surface_marker(pose, dimensions):
     ''' Function that generates a surface marker'''
     int_marker = InteractiveMarker()
     int_marker.name = 'surface'
     int_marker.header.frame_id = 'base_link'
     int_marker.pose = pose
     int_marker.scale = 1
     button_control = InteractiveMarkerControl()
     button_control.interaction_mode = InteractiveMarkerControl.BUTTON
     button_control.always_visible = True
     object_marker = Marker(type=Marker.CUBE, id=2000,
                         lifetime=rospy.Duration(2),
                         scale=dimensions,
                         header=Header(frame_id='base_link'),
                         color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
                         pose=pose)
     button_control.markers.append(object_marker)
     text_pos = Point()
     position = pose.position
     dimensions = dimensions
     text_pos.x = position.x + dimensions.x / 2 - 0.06
     text_pos.y = position.y - dimensions.y / 2 + 0.06
     text_pos.z = position.z + dimensions.z / 2 + 0.06
     text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
             scale=Vector3(0, 0, 0.03), text=int_marker.name,
             color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
             header=Header(frame_id='base_link'),
             pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
     button_control.markers.append(text_marker)
     int_marker.controls.append(button_control)
     return int_marker

def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"):
    marker = Marker()
    marker.header.frame_id = frame_id
    marker.type = marker.CUBE
    marker.id = marker_id
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[2]
    marker.color.a = rgba[3]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
    obj_control = InteractiveMarkerControl()
    obj_control.always_visible = True
    obj_control.markers.append( marker )
        
    return obj_control

    def _get_object_marker(self, index, mesh=None):
        '''Generate a marker for world objects'''
        int_marker = InteractiveMarker()
        int_marker.name = World.objects[index].get_name()
        int_marker.header.frame_id = 'base_link'
        int_marker.pose = World.objects[index].object.pose
        int_marker.scale = 1

        button_control = InteractiveMarkerControl()
        button_control.interaction_mode = InteractiveMarkerControl.BUTTON
        button_control.always_visible = True

        object_marker = Marker(type=Marker.CUBE, id=index,
                lifetime=rospy.Duration(2),
                scale=World.objects[index].object.dimensions,
                header=Header(frame_id='base_link'),
                color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
                pose=World.objects[index].object.pose)

        if (mesh != None):
            object_marker = World._get_mesh_marker(object_marker, mesh)
        button_control.markers.append(object_marker)

        text_pos = Point()
        text_pos.x = World.objects[index].object.pose.position.x
        text_pos.y = World.objects[index].object.pose.position.y
        text_pos.z = (World.objects[index].object.pose.position.z +
                     World.objects[index].object.dimensions.z / 2 + 0.06)
        button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                id=index, scale=Vector3(0, 0, 0.03),
                text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                header=Header(frame_id='base_link'),
                pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
        int_marker.controls.append(button_control)
        return int_marker

def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
    marker = Marker()
    marker.mesh_resource = resource;
    marker.header.frame_id = frame_id
    marker.type = marker.MESH_RESOURCE
    marker.scale.x = scale*scales[0]
    marker.scale.y = scale*scales[1]
    marker.scale.z = scale*scales[2]
    marker.color.a = rgba[3]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
    obj_control = InteractiveMarkerControl()
    obj_control.always_visible = True
    obj_control.markers.append( marker )
        
    return obj_control

 def MakeMuneObject(self, MenuName, MenuPose):
 
  MenuInteractiveMarker = InteractiveMarker()
  MenuInteractiveMarker.name = MenuName
  MenuInteractiveMarker.header.frame_id = self.frame_id
  MenuInteractiveMarker.pose.position.z += self.MenuHight
  MenuInteractiveMarker.scale = self.MenuScale
  
  MenuControl = InteractiveMarkerControl()
  MenuControl.interaction_mode = InteractiveMarkerControl.MENU
  MenuControl.always_visible = False
  
  MenuMarker = Marker()
  
  MenuMarker.type = Marker.ARROW
  MenuMarker.scale.x = MenuInteractiveMarker.scale * 2
  MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45
  MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45
  MenuMarker.color.r = 0.5
  MenuMarker.color.g = 0.5
  MenuMarker.color.b = 0.5
  MenuMarker.color.a = 1.0
  MenuMarker.pose = MenuPose
    
  MenuControl.markers.append(MenuMarker)
  
  MenuInteractiveMarker.controls.append(MenuControl)
  
  #print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker
  
  self.server.insert(MenuInteractiveMarker)
  rospy.loginfo('insert Menu Marker Object')

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 MessControl(self, unit, mode):
  control=InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.y = 1
  control.interaction_mode= mode
  control.always_visible=True
  control.markers.append(copy.deepcopy(unit))

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 update_viz(self):
     
     menu_control = InteractiveMarkerControl()
     menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
     menu_control.always_visible = True
     frame_id = 'base_link'
     pose = self.ee_pose
     
     menu_control = self._add_gripper_marker(menu_control)
     text_pos = Point()
     text_pos.x = pose.position.x
     text_pos.y = pose.position.y
     text_pos.z = pose.position.z + 0.1
     text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
     menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                                        id=0, scale=Vector3(0, 0, 0.03),
                                        text=text,
                                        color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                                        header=Header(frame_id=frame_id),
                                        pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
     int_marker = InteractiveMarker()
     int_marker.name = 'ik_target_marker'
     int_marker.header.frame_id = frame_id
     int_marker.pose = pose
     int_marker.scale = 0.2
     self._add_6dof_marker(int_marker)
     int_marker.controls.append(menu_control)
     self._im_server.insert(int_marker, self.marker_clicked_cb)

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 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 create_object_marker(self, soma_obj, soma_type, pose):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = soma_obj
        int_marker.description = "id" + soma_obj
        int_marker.pose = pose
        
        mesh_marker = Marker()
        mesh_marker.type = Marker.MESH_RESOURCE
        mesh_marker.scale.x = 1
        mesh_marker.scale.y = 1
        mesh_marker.scale.z = 1

        random.seed(soma_type)
        val = random.random()
        mesh_marker.color.r = r_func(val)
        mesh_marker.color.g = g_func(val)
        mesh_marker.color.b = b_func(val)
        mesh_marker.color.a = 1.0
        #mesh_marker.pose = pose
        mesh_marker.mesh_resource = self.mesh[soma_type]

        # 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.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE

        if self._interactive:
            int_marker.controls.append(copy.deepcopy(control))
            # add the control to the interactive marker
            int_marker.controls.append(control);

        # add menu control
        menu_control = InteractiveMarkerControl()

        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        
        menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
        int_marker.controls.append(menu_control)

        return int_marker

    def create_object_marker(self, soma_obj, roi, soma_type, pose):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "map"
        int_marker.name = soma_obj
        int_marker.description = soma_type + ' (' + roi +  ')'
        int_marker.pose = pose
        int_marker.pose.position.z = 0.01
        
        marker = Marker()
        marker.type = Marker.SPHERE
        marker.scale.x = 0.25
        marker.scale.y = 0.25
        marker.scale.z = 0.25
        int_marker.pose.position.z = (marker.scale.z / 2)
        
        random.seed(soma_type)
        val = random.random()
        marker.color.r = r_func(val)
        marker.color.g = g_func(val)
        marker.color.b = b_func(val)
        marker.color.a = 1.0
        #marker.pose = pose
        # 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.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE

        if self._interactive:
            int_marker.controls.append(copy.deepcopy(control))
            # add the control to the interactive marker
            int_marker.controls.append(control);

        # add menu control
        menu_control = InteractiveMarkerControl()

        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        
        menu_control.markers.append( marker) #makeBox(int_marker) )
        int_marker.controls.append(menu_control)

        return int_marker

 def create_im(self, marker, pose, name):
     # create the new interactive marker
     int_marker = InteractiveMarker()
     int_marker.pose = copy.deepcopy(pose)
     int_marker.header.frame_id = 'base_link'
     int_marker.name = name
     # move freely on the X-Y plane
     control = InteractiveMarkerControl()
     control.orientation.w = 1
     control.orientation.x = 0
     control.orientation.y = 1
     control.orientation.z = 0
     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
     control.markers.append(marker)
     control.always_visible = True
     int_marker.controls.append(control)
     return int_marker

 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 create_interactive_marker(holder, id, pos, size, color, func):
    # Make interactive marker for mouse selection
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base"
    int_marker.name = "object"+str(id)
    int_marker.pose.position.x = pos[0]
    int_marker.pose.position.y = pos[1]
    int_marker.pose.position.z = pos[2]

    #color = [1, 0, 0]

    # Add click control
    box_control = InteractiveMarkerControl()
    box_control.always_visible = True
    box_control.interaction_mode = InteractiveMarkerControl.BUTTON

    create_shape(box_control, "object", id, pos, size=size, color=color)

    # add the control to the interactive marker
    int_marker.controls.append( box_control )
    holder.insert(int_marker, lambda x: func(x, color))

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 create_roi_marker(self, roi, soma_type, pose, points):
        #print "POINTS: " + str(points)
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "map"
        int_marker.name = "ROI-" + roi
        int_marker.description = roi
        int_marker.pose = pose
        
        marker = Marker()
        marker.type = Marker.LINE_STRIP
        marker.scale.x = 0.1
        
        random.seed(soma_type)
        val = random.random()
        marker.color.r = r_func(val)
        marker.color.g = g_func(val)
        marker.color.b = b_func(val)
        marker.color.a = 1.0

        control = InteractiveMarkerControl()
        control.always_visible = True
        control.markers.append( marker )

        int_marker.controls.append(control )
        
        marker.points = []
        for point in points:
            p = Point()
            pose = self._soma_obj_pose[point]
            p.x = pose.position.x - int_marker.pose.position.x  
            p.y = pose.position.y - int_marker.pose.position.y
            marker.points.append(p)

        p = Point()
        pose = self._soma_obj_pose[points[0]]
        p.x = pose.position.x - int_marker.pose.position.x  
        p.y = pose.position.y - int_marker.pose.position.y
        marker.points.append(p)

        return int_marker

 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