def __init__(self, publish_name, xWid,yHei):
        self.tf_listener = tf.TransformListener()
        self.tf_name = ''

        self.bb_publisher = rospy.Publisher(publish_name, Convex_Space)
        self.marker_pub = rospy.Publisher('/look_for_this/found_it/bounding_pyramids/markers', Marker)
        
        self.xWid = xWid
        self.yHei = yHei

        self.aspectXtoZ = 1.0
        self.aspectYtoZ = 1.0
        
        self.K_dict = {}
        
       	global marker
	marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.Time()
        marker.ns = ""
        marker.id = 0
        marker.type = 5 # 5 = LINELIST
        marker.action = 0 # 0 = ADD
        marker.pose.position.x = 0
        marker.pose.position.y = 0
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 0.0
        marker.scale.x = 0.01
        marker.color.a = 1
        marker.color.r = 0
        marker.color.g = 0
        marker.color.b = 0

    def __init__(self):
        self.layout = rospy.get_param('/thruster_layout/thrusters')  # Add thruster layout from ROS param set by mapper
        assert self.layout is not None, 'Could not load thruster layout from ROS param'

        '''
        Create MarkerArray with an arrow marker for each thruster at index node_id.
        The position of the marker is as specified in the layout, but the length of the arrow
        will be set at each thrust callback.
        '''
        self.markers = MarkerArray()
        for i in xrange(len(self.layout)):
            # Initialize each marker (color, scale, namespace, frame)
            m = Marker()
            m.header.frame_id = '/base_link'
            m.type = Marker.ARROW
            m.ns = 'thrusters'
            m.color.a = 0.8
            m.scale.x = 0.01  # Shaft diameter
            m.scale.y = 0.05  # Head diameter
            m.action = Marker.DELETE
            m.lifetime = rospy.Duration(0.1)
            self.markers.markers.append(m)
        for key, layout in self.layout.iteritems():
            # Set position and id of marker from thruster layout
            idx = layout['node_id']
            pt = numpy_to_point(layout['position'])
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].id = idx

        # Create publisher for marker and subscribe to thrust
        self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
        self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)

def publish(anns, data):
    wall_list = WallList()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # Walls
        object = deserialize_msg(d.data, Wall)
        wall_list.obstacles.append(object)
        
        # Markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "wall_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('wall_marker',    MarkerArray, latch=True, queue_size=1)
    wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch=True, queue_size=1)

    wall_pub.publish(wall_list)
    marker_pub.publish(marker_list)
    
    return

 def publish_prob2(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     idx = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs) 
     current_probs = [0 for foo in objects]
     for node in self._topo_map:
         if node.name in waypoints:
             for j in range(0, n_objects):
                 marker = Marker()
                 marker.header.frame_id = 'map'
                 marker.id = idx
                 marker.type = Marker.CYLINDER
                 marker.action = Marker.ADD
                 marker.pose = node.pose
                 prob = probs[n_objects*i + j]
                 prob = prob/(scaling_factor)
                 print "AHAHHAHBHBHBHBHBHB", prob
                 marker.pose.position.z  = marker.pose.position.z + current_probs[j]
                 marker.scale.x = 1*prob
                 marker.scale.y = 1*prob                
                 marker.scale.z = 1*prob
                 current_probs[j] = current_probs[j] + prob + 0.1
                 marker.color.a = 1.0
                 marker.color.r = 1.0*prob
                 marker.color.g = 1.0*prob
                 marker.color.b = 1.0*prob
                 prob_msg.markers.append(marker)
                 idx = idx + 1
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)    

   def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):

      marker_array = MarkerArray()

      for i in xrange(len(p_arr)):
	 p = p_arr[i]
	 v = vx,vy,vz = v_arr[i]
	 marker = Marker()
	 marker.header.frame_id = "/openni_rgb_optical_frame"
	 marker.type = marker.ARROW
	 marker.action = marker.ADD
	 marker.color.a = 1.0
	 marker.color.r = 1.0
	 marker.color.g = 0.0
	 marker.color.b = 0.0
	 marker.points.append(Point(p[0],p[1],p[2]))
	 marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) 
	 marker.scale.x=0.05
	 marker.scale.y=0.1
	 marker.id = i 

	 marker_array.markers.append(marker)


      self.marker_pub.publish(marker_array)

    def __setMarker(self, id, waypoint, colors = [1,0,0,0]):
        try:
            marker = Marker()
            marker.header.frame_id = '/map'
            marker.header.stamp = rospy.Time.now()
            marker.ns = 'patrol'
            marker.id = id
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            marker.color.a = colors[0]
            marker.color.r = colors[1]
            marker.color.b = colors[2]
            marker.color.g = colors[3]

            marker.pose.orientation.w = 1.0
            marker.pose.position.x = waypoint.target_pose.pose.position.x
            marker.pose.position.y = waypoint.target_pose.pose.position.y
            marker.pose.position.z = waypoint.target_pose.pose.position.z

            return marker

        except Exception as ex:
            rospy.logwarn('PatrolNode.__setMarker- ', ex.message)

	def publish_marker(self):
		# create marker
		marker = Marker()
		marker.header.frame_id = "/base_link"
		marker.header.stamp = rospy.Time.now()
		marker.ns = "color"
		marker.id = 0
		marker.type = 2 # SPHERE
		marker.action = 0 # ADD
		marker.pose.position.x = 0
		marker.pose.position.y = 0
		marker.pose.position.z = 1.5
		marker.pose.orientation.x = 0.0
		marker.pose.orientation.y = 0.0
		marker.pose.orientation.z = 0.0
		marker.pose.orientation.w = 1.0
		marker.scale.x = 0.1
		marker.scale.y = 0.1
		marker.scale.z = 0.1
		marker.color.a = self.color.a #Transparency
		marker.color.r = self.color.r
		marker.color.g = self.color.g
		marker.color.b = self.color.b
		# publish marker
		self.pub_marker.publish(marker)

	def draw_base(self):
		marker = Marker()
		ratio = (self.rod_position[CENTER][1] - self.rod_position[LEFT][1]) / (self.rod_position[CENTER][0] - self.rod_position[LEFT][0]) 
		alpha = numpy.arctan(ratio) - numpy.pi / 2.0

		marker.header.frame_id = "/pl_base"
		marker.ns = "basic_shapes"
		marker.id = 5
		marker.type = marker.CUBE
		marker.action = marker.ADD
		marker.pose.position.x = self.rod_position[CENTER][0]
		marker.pose.position.y = self.rod_position[CENTER][1]
		marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE
		marker.pose.orientation.x = 0.0
		marker.pose.orientation.y = 0.0		
		marker.pose.orientation.z = numpy.sin(alpha / 2.0)
		marker.pose.orientation.w = numpy.cos(alpha / 2.0) 		
		marker.scale.x = BASE_SCALE_XYZ[0]
		marker.scale.y = BASE_SCALE_XYZ[1]
		marker.scale.z = BASE_SCALE_XYZ[2]
		marker.color.r = ROD_COLOR_RGB[0]
		marker.color.g = ROD_COLOR_RGB[1]
		marker.color.b = ROD_COLOR_RGB[2]
		marker.color.a = 1.0

		self.rviz_pub.publish(marker)

def createMarkerLine(pos_list, color = (.1, .1, 1), ID = 0, alpha = 1., size = 0.5):
    marker = Marker()
    marker.header.frame_id = "/openni_depth_frame"
    marker.id = ID
    marker.type = marker.LINE_STRIP
    marker.action = marker.ADD

    marker.scale.x = size
    marker.scale.y = size
    marker.scale.z = size
    marker.color.a = alpha

    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.pose.orientation.w = 1.0

    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0

    for p in pos_list:
        marker.points.append( Point(p[0],p[1],0) )

    return marker

    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]

        control = InteractiveMarkerControl()
        control.markers.append(mesh_marker)
        int_marker.controls.append(control)
        
        return int_marker

    def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
        """
        line: t r + p

        This will be drawn for t[0] .. t[1]
        """

        r = np.array(r)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'lines'
        m.id = 0 if key else len(self.lines)
        m.type = Marker.LINE_STRIP
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(0.005, 0.005, 0.005)
        m.color = ColorRGBA(0,0.8,0.8,1)

        m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
        m.colors = [m.color] * 10

        key = key or element_key(m)

        with self.mod_lock:
            self.lines[key] = m

        return key

    def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0):
        (mesh, pose) = self.get_link_data(link)

        marker = Marker()

        if offset==None :
            marker.pose = pose
        else :
            marker.pose = toMsg(fromMsg(offset)*fromMsg(pose))

        marker.header.frame_id = root
        marker.header.stamp = rospy.get_rostime()
        marker.ns = self.robot_name
        marker.mesh_resource = mesh
        marker.type = Marker.MESH_RESOURCE
        marker.action = Marker.MODIFY
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = color[3]
        marker.text = link
        marker.id = idx
        marker.mesh_use_embedded_materials = True
        return marker

 def _delete_markers(self):
     marker = Marker()
     marker.action = 3
     marker.header.frame_id = "map"
     markerarray = MarkerArray()
     markerarray.markers.append(marker)
     self.markerpub.publish(markerarray)

    def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno):
        # create an interactive marker for our server
        marker = Marker()
        marker.header.frame_id = "map"
        #int_marker.name = soma_obj+'_'+str(markerno)
       #int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+  ')'
        marker.pose = pose
        marker.id = markerno;
       # print marker.pose
        marker.pose.position.z = 0.01


        #marker = Marker()
        marker.type = Marker.SPHERE
        marker.action = 0
        marker.scale.x = 0.25
        marker.scale.y = 0.25
        marker.scale.z = 0.25
        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

        return marker

 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 createArrowMarker(points, color, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.ARROW
    marker.scale.x = 0.003
    marker.scale.y = 0.005
    marker.scale.z = 0
    
    n = len(points)//3
    marker.id = 2
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = color[3]
        
    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]
    
        
    return marker

def publish(anns, data):
    ar_mk_list = AlvarMarkers()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # AR markers
        object = deserialize_msg(d.data, AlvarMarker)
        ar_mk_list.markers.append(object)
        
        # Visual markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "ar_mk_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('ar_mk_marker',    MarkerArray, latch=True, queue_size=1)
    ar_mk_pub  = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)

    ar_mk_pub.publish(ar_mk_list)
    marker_pub.publish(marker_list)
    
    return

def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.SPHERE
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[2]
    marker.id = 1
        
    p = ColorRGBA()
    p.r = color[0]
    p.g = color[1]
    p.b = color[2]
    p.a = color[3]
    marker.color = p
        
    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]
    
        
    return marker

	def set_position_callback(self,marker,joy):
		print self.position_marker.ns
		print joy.buttons[3] == 1
		print marker.ns == "PEOPLE"
		if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"):
			self.position_marker = marker
			print "set position"
			ref_marker = Marker()
			ref_marker.header.frame_id = "base_footprint"
			ref_marker.header.stamp = rospy.get_rostime()
			ref_marker.ns = "robot"
			ref_marker.type = 9
			ref_marker.action = 0
			ref_marker.pose.position.x = self.position_marker.pose.position.x
			ref_marker.pose.position.y = self.position_marker.pose.position.y
			ref_marker.pose.position.z = self.position_marker.pose.position.z
			ref_marker.scale.x = .25
			ref_marker.scale.y = .25
			ref_marker.scale.z = .25
			ref_marker.color.r = 1.0
			ref_marker.color.g = 0.0
			ref_marker.color.a = 1.0
			ref_marker.lifetime = rospy.Duration(0)
			self.ref_viz_pub.publish(ref_marker)
		else:
			pass

def createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.POINTS
    marker.scale.x = 0.003
    marker.scale.y = 0.003
    marker.scale.z = 0.003
    marker.id = 1
    
    n = len(points)//3
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
        p = ColorRGBA()
        p.r = colors[i*3+0]
        p.g = colors[i*3+1]
        p.b = colors[i*3+2]
        p.a = 0.5
        marker.colors.append(p)
        
    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]
    
        
    return marker