def __init__(self):

        super(CCA, self).__init__()

        #UIの初期化
        self.initUI()

        #ROSのパブリッシャなどの初期化
        rospy.init_node('roscca', anonymous=True)
        self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
        self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)

        #rvizのカラー設定(未)
        self.carray = []
        clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
        for c in clist:
            color = ColorRGBA()
            color.r = c[0]
            color.g = c[1]
            color.b = c[2]
            color.a = c[3]
            self.carray.append(color) 

        self.fnames=["20151222_a1.json","20151222_a2.json","20151222_a3.json","20151222_b1.json","20151222_b2.json","20151222_b3.json"]
        #self.fnames=["20151222_a1.json"]
        self.winSizes = [90]

def main():
    display_pub = rospy.Publisher('leds/display', Keyframe, queue_size=10)
    anim_pub = rospy.Publisher('leds/animation', Animation, queue_size=10)
    set_pub = rospy.Publisher('leds/set', Command, queue_size=10)

    rospy.init_node('led_anim_clear', anonymous = True)
    time.sleep(0.5)

    keyframe_msg = Keyframe()
    command_msg = Command()
    color_msg = ColorRGBA()
    anim_msg = Animation()
    
    rospy.loginfo("Single frame test ...")
    keyframe_msg.color_pattern = []
    color_msg.r = 0.0
    color_msg.g = 0.0
    color_msg.b = 0.0
    keyframe_msg.color_pattern.append(copy.deepcopy(color_msg))
    display_pub.publish(keyframe_msg)
    time.sleep(0.5)

    rospy.loginfo("Clearing the animation...")    
    anim_msg.iteration_count = 1
    anim_pub.publish(anim_msg)
    time.sleep(1)

	def MoveToBlob(self,msg): #Moves to the largest blob
		drivemsg = AckermannDriveStamped() #Sets the drive messaged to the AckermannDriveStamped message type
		drivemsg.drive.speed = 2	#Sets a field of the drivemsg message.

				
		#Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size)

		ele = ColorRGBA()
		ele.r = 0
		ele.g = 255
		largest_green_blob_index = self.blob_detect.colors.index(ele)

		blob_pos = self.blob_detec.locations[largest_green_bloc_index]  #Get's the position of the largest green block					
		blob_x_pos = blob_pos[0]
		blob_y_pos = blob_pos[1]
		
		biggest_blob_area = blob_detec.sizes[0] #The blob with index one is the biggest blob
		
		if (biggest_blob_area < 1000):	#Keep moving	
			if (blob_x_pos > self.img_width/2): 	#If Blob is On the right side
				drivemsg.drive.steering_angle = .33 #Turn left
			if (blob_x_pos < self.img_width/2): #If Blob is on the left side.
				drivemsg.drive.steering_angle = -0.33 #Turn right
			self.pub_mov_func.publish(drivemsg)
		else:	# if the contour is really big, we are close to the sign, so we do an emergency stop. We can delete this if we were to use the lidar.
			drivemsg.drive.speed = 0
			self.pub_mov_func.publish(drivemsg)

	def MoveToBlob(self): #Moves to the largest [choose a color] blob
		self.drive_msg.drive.speed = 2	#Sets a field of the drivemsg message.
				
		#Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size). The next three lines
		#define a specific color that we want to find/move to
		ele = ColorRGBA()
		ele.r = 0
		ele.g = 255
		largest_green_blob_index = self.blob_detect.colors.index(ele)
		blob_pos = self.blob_detect.locations[largest_green_blob_index]  
					
		blob_x_pos = blob_pos[0]		#This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right (?))
		blob_y_pos = blob_pos[1]		#Same thing here
		
		blob_area = blob_detect.size[largest_green_blob_index] #Get the area of the largest block of the set of the blocks with the color we specified
		

		if (blob_area < 1000):	#Keep moving unless the blob_area becomes too big (meaning that we must be very close to the blob, ie it is taking up more of our field of vision.
			#If the blob_area is "small, we want to keep moving toward the blob while continually centering our field of vision with the blob.
			if (blob_x_pos > 0.55): #If Blob is On the right side
				drive_msg.drive.steering_angle = .33 #Turn left
			elif (blob_x_pos < 0.45): #If Blob is on the left side.
				drive_msg.drive.steering_angle = -0.33 #Turn right
			else:	#If blob_x_pos (the x component of the blob's centroid) is in the middle 200 pixels, just move forward
				drive_msg.drive.steering_angle = 0
			self.pub_mov_func.publish(drive_msg)
		else:	# if the contour is really big, we are close to the sign, so we do an emergency stop. This is a bit redundant with our emergency stop function that comes from the lidar scan.
			self.drive_msg.drive.speed = 0
			self.pub_mov_func.publish(drive_msg)

    def find_color(self, im, label_color, mask):
        contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
        #cv2.drawContours(im, contours, -1, (255, 255, 255), 2)
        approx_contours = []
        for c in contours:
	    area = cv2.contourArea(c)
            if area < 100: continue
            perim = cv2.arcLength(c, True)
            approx = cv2.approxPolyDP(c, .05*perim, True)
            #if len(approx) == 4 and len(cv2.convexityDefects(c, cv2.convexHull(c))) <= 1:
            if len(approx) == 4:
                approx_contours.append(approx)
                moments = cv2.moments(c)
                center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
                cv2.circle(im, center, 3, (255, 100, 100), 4)
                print "Moment:  ({}, {})".format(center[0], center[1])

                msg_color = ColorRGBA()
                msg_color.r, msg_color.g, msg_color.b = label_color
                self.msg.colors.append(msg_color)
                print "Label color:  {}".format(label_color)
                msg_size = Float64()
                #msg_size.data = max(math.sqrt((approx[1][0][0]-approx[0][0][0])**2+(approx[1][0][1]-approx[0][0][1])**2), math.sqrt((approx[2][0][0]-approx[1][0][0])**2+(approx[2][0][1]-approx[2][0][0])**2))
                msg_size.data = float((max(approx, key=lambda x: x[0][0])[0][0] - min(approx, key=lambda x: x[0][0])[0][0])) / len(im[0])
                print "Width:  {}".format(msg_size.data)
                self.msg.sizes.append(msg_size)
                msg_loc = Point()
                msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
                self.msg.locations.append(msg_loc)

    def __init__(self):

        super(CCA, self).__init__()

        #UIの初期化
        self.initUI()

        #ファイル入力
        #self.jsonInput()

        #ROSのパブリッシャなどの初期化
        rospy.init_node('ros_cca_table', anonymous=True)
        self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
        self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)


        self.carray = []
        clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
        for c in clist:
            color = ColorRGBA()
            color.r = c[0]
            color.g = c[1]
            color.b = c[2]
            color.a = c[3]
            self.carray.append(color) 

 def to_msg(self):
     msg = ColorRGBA()
     msg.r = self.r / 255.0
     msg.g = self.g / 255.0
     msg.b = self.b / 255.0
     msg.a = 1.0
     return msg

    def __init__(self):

        super(CCA, self).__init__()

        #UIの初期化
        self.initUI()

        #クラス間のデータ渡しテスト
        self.test = 100

        #ROSのパブリッシャなどの初期化
        rospy.init_node('roscca', anonymous=True)
        self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
        self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)

        #rvizのカラー設定(未)
        self.carray = []
        clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
        for c in clist:
            color = ColorRGBA()
            color.r = c[0]
            color.g = c[1]
            color.b = c[2]
            color.a = c[3]
            self.carray.append(color) 

def hex_to_color_msg(hex_str):
    rgb = struct.unpack('BBB', hex_str.decode('hex'))
    msg = ColorRGBA()
    msg.r = rgb[0]
    msg.g = rgb[1]
    msg.b = rgb[2]
    msg.a = 1
    return msg

 def get_color(self, obj):
     if obj in self.objects:
         return self.objects[obj]
     else:
         c = ColorRGBA()
         c.r, c.g, c.b = self.COLORS[len(self.objects) % len(self.COLORS)]
         c.a = 1.0
         self.objects[obj] = c
         return c

 def robot_position_marker(self):
  color = ColorRGBA()
  scale = Point()
  scale.x = 0.1
  scale.y = 0.1
  scale.z = 0.1
  color.r = 1.0
  color.a = 1.0
  self.robot_position = self.visual_test(self.pose, Marker.CUBE, color, scale) 

 def static_area_makers(self):
  color = ColorRGBA()
  scale = Point()
  scale.x = 0.05
  scale.y = 0.05
  color.r = 1.0
  color.g = 1.0
  color.b = 0.0
  color.a = 1.0
  self.points_marker = self.visual_test(self.static_area, Marker.POINTS, color, scale)

    def create_trajectory_marker(self, traj):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = traj.uuid
        int_marker.description = traj.uuid
        pose = Pose()
        pose.position.x = traj.pose[0]['position']['x']
        pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose

        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.05

        # random.seed(traj.uuid)
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []

        MOD  = 1
        for i, point in enumerate(traj.pose):
            if i % MOD == 0:
                x = point['position']['x']
                y = point['position']['y']
                p = Point()
                p.x = x - int_marker.pose.position.x  
                p.y = y - int_marker.pose.position.y
                line_marker.points.append(p)

        line_marker.colors = []
        for i, vel in enumerate(traj.vel):
            if i % MOD == 0:
                color = ColorRGBA()
                val = vel / traj.max_vel
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

                
        # 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.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker

	def set_light(self,parameter_name,blocking=False):
		ah = action_handle("set", "light", parameter_name, blocking, self.parse)
		if(self.parse):
			return ah
		else:
			ah.set_active(mode="topic")

		rospy.loginfo("Set light to <<%s>>",parameter_name)
		
		# get joint values from parameter server
		if type(parameter_name) is str:
			if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
				rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
				return 2
			param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
		else:
			param = parameter_name
			
		# check color parameters
		if not type(param) is list: # check outer list
			rospy.logerr("no valid parameter for light: not a list, aborting...")
			print "parameter is:",param
			ah.error_code = 3
			return ah
		else:
			if not len(param) == 3: # check dimension
				rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
				print "parameter is:",param
				ah.error_code = 3
				return ah
			else:
				for i in param:
					#print i,"type1 = ", type(i)
					if not ((type(i) is float) or (type(i) is int)): # check type
						#print type(i)
						rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
						print "parameter is:",param
						ah.error_code = 3
						return ah
					else:
						rospy.logdebug("accepted parameter %f for light",i)
		
		# convert to ColorRGBA message
		color = ColorRGBA()
		color.r = param[0]
		color.g = param[1]
		color.b = param[2]
		color.a = 1 # Transparency

		# publish color		
		self.pub_light.publish(color)
		
		ah.set_succeeded()
		ah.error_code = 0
		return ah

 def clear_area_makers(self):
  color = ColorRGBA()
  scale = Point()
  scale.x = 0.05
  scale.y = 0.05
  color.r = 0.0
  color.g = 1.0
  color.b = 0.0
  color.a = 1.0
  self.ClearPoints_marker = self.visual_test(self.clear_area, Marker.POINTS, color, scale)
  print 'finish build markers', len(self.ClearPoints_marker.points)

 def LaserDataMarker(self,LaserData): ####################visual_test
  color=ColorRGBA()
  scale=Point()
  scale.x=0.04
  scale.y=0.04
  color.r=0.0
  color.g=2.0
  color.b=0.0
  color.a=1.0
  
  self.laser_points_marker=self.visual_test(LaserData, Marker.POINTS, color, scale)

 def flag_makers(self):
  color = ColorRGBA()
  scale = Point()
  pose = Pose()
  scale.x = 0.01
  scale.y = 0.01
  scale.z = 0.2
  pose = self.pose
  pose.position.z += 0.5
  color.r = 1.0
  color.a = 1.0
  self.flag_marker = self.visual_test(pose, Marker.TEXT_VIEW_FACING, color, scale)

    def create_plan_marker(self, plan, plan_values):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = plan.ID
        int_marker.description = plan.ID
        pose = Pose()
        #pose.position.x = traj.pose[0]['position']['x']
        #pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose
        
        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.1

        # random.seed(float(plan.ID))
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []
        for view in plan.views:
            x = view.get_ptu_pose().position.x
            y = view.get_ptu_pose().position.y
            z = 0.0 # float(plan.ID) / 10
            p = Point()
            p.x = x - int_marker.pose.position.x  
            p.y = y - int_marker.pose.position.y
            p.z = z - int_marker.pose.position.z
            line_marker.points.append(p)

            line_marker.colors = []
            for i, view in enumerate(plan.views):
                color = ColorRGBA()
                val = float(i) / len(plan.views)
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

        # 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.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker

 def __init__(self):
     #ROSのパブリッシャなどの初期化
     #rospy.init_node('ccaviz', anonymous=True)
     self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
     self.carray = []
     clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
     for c in clist:
         color = ColorRGBA()
         color.r = c[0]
         color.g = c[1]
         color.b = c[2]
         color.a = c[3]
         self.carray.append(color)

def createPointLine (marker, d=0.1, n=50.0):
        
    t = 0.0
    while t < d:
        p = Point()
        p.z = t
        c = ColorRGBA()
        c.b, c.a = 1, 0.5
        
        marker.points.append(p)
        marker.colors.append(c)
        
        t += d/n