def __init__(self):
	global pubForPos;
        """Read config file and setup ROS things"""
        self.config = read_config()
        self.config["prob_move_correct"] = .75
        rospy.init_node("map_server")
        self.map_data_service = rospy.Service(
                "requestMapData",
                requestMapData,
                self.handle_data_request
        )
        self.move_service = rospy.Service(
                "moveService",
                moveService,
                self.handle_move_request
        )
        self.pos = self.initialize_position()

	something=positionMessage()
	something.position=deepcopy(self.pos)
	pubForPos.publish(something)

        r.seed(self.config['seed'])
        print "starting pos: ", self.pos
        rospy.spin()

def colorFilter():
	global localMap, truePos
	colorMap   =[
        ["B","R","R","B","B"],
        ["R","R","B","B","R"],
        ["R","B","R","R","R"],
        ["R","B","B","B","R"]
        ]
	realColor  =colorMap[truePos[0]][truePos[1]]
	roll       = r.uniform(0,1)
	correctProb=read_config()["prob_color_correct"]
	if roll > correctProb:
	        if realColor == 'B':
	                realColor='R'
	        else:
	                realColor='B'
	for i in range (len(localMap)):
		for j in range (len(localMap[0])):
			if colorMap[i][j]==realColor:
			        localMap[i][j]*=correctProb
			else:
			        localMap[i][j]*=(1-correctProb)
	totalSum=0
	for j in range (0, len(localMap)):
		for k in range (0, len(localMap[0])):
			totalSum+=localMap[j][k]


#print "totalSum: ", totalSum
	for j in range (0, len(localMap)):
		for k in range (0, len(localMap[0])):
			localMap[j][k]/=totalSum

def computeValueFromQValues(curr_position):
    """
      Returns max_action Q(state,action)
      where the max is over legal actions.  Note that if
      there are no legal actions, which is the case at the
      terminal state, you should return a value of 0.0.
    """
    # Set max value to negative infinity
    max_value = float("-inf")
    config = read_config()
    move_list = config['move_list']
    legalActions = []

    for move in move_list:
        if validMove(curr_position, move):
            legalActions.append(move)
    if len(legalActions) == 0:
        return 0.0
    else:
        # Compute max Q-Value
        for action in legalActions:
            qValue = getQValue(curr_position, action)
            if qValue > max_value:
                max_value = qValue

    # Return max Q-Value
    return max_value

    def bootstrap(self):
        self.config      = read_config()
        self.texture_map = [item for sublist in \
                self.config["texture_map"] for item in sublist]

        self.pipe_map    = [item for sublist in \
                self.config["pipe_map"] for item in sublist]

        self.rows        = len(self.config["texture_map"])
        self.cols        = len(self.config["texture_map"][0])
        self.grid_size   = self.rows*self.cols

        self.temp_dict   = {
                'H': 40.0,
                'C': 20.0,
                '-': 25.0
        }

        self.prob_move_correct   = self.config["prob_move_correct"]
        self.prob_move_incorrect = (1. - self.prob_move_correct) / 4.

        self.prob_tex_correct    = self.config["prob_tex_correct"]
        self.prob_tex_incorrect  = 1. - self.prob_tex_correct

        self.temp_std_dev        = self.config["temp_noise_std_dev"]

        self.move_list           = self.config["move_list"]
        self.move_num            = 0
        self.move_max            = len(self.move_list)

        self.belief              = np.full(self.grid_size, 1./self.grid_size)

	def __init__(self):
		self.json_data = read_config()
		self.cost = 1
		self.avoid_states = [AStar.OBSTACLE, AStar.PIT]
		self.create_pub()
		self.create_map()
		self.astar()

    def __init__(self):
        # read the config
        random.seed(0)
        config = read_config()
        self.goal = config['goal']
        self.walls = config['walls']
        self.pits = config['pits']
        self.map_size = config['map_size']
        self.alpha = config['alpha']
        self.constant = config['constant']
        self.maxL = config['maxL']

        self.reward_for_reaching_goal = config['reward_for_reaching_goal']
        self.reward_for_falling_in_pit = config['reward_for_falling_in_pit']
        self.reward_for_hitting_wall = config['reward_for_hitting_wall']
        self.reward_for_each_step = config['reward_for_each_step']

        self.max_iterations = config['max_iterations']
        self.threshold_difference = config['threshold_difference']
        self.discount_factor = config['discount_factor']

        self.uncertainty = config['uncertainty']
        prob_move_forward = config['prob_move_forward']
        prob_move_backward = config['prob_move_backward']
        prob_move_left = config['prob_move_left']
        prob_move_right = config['prob_move_right']
        self.prob_move = [prob_move_forward, prob_move_backward, prob_move_left, prob_move_right]

        self.qvalues = [[Grid() for i in range(self.map_size[1])] for j in range(self.map_size[0])]
        self.generate_intermedia_video = config['generate_intermedia_video']
        self.video_iteration = 0

    def __init__(self):
        self.config = read_config()
        rospy.init_node('robot')

        self.temp_service = rospy.Publisher("/temp_sensor/activation", Bool, queue_size = 10)
        rospy.Subscriber("/temp_sensor/data", temperatureMessage, self.temp_callback)

        self.texService = self.request_texture_service() 
        self.movService = self.request_move_service() 

        self.pos_out = rospy.Publisher("/results/probabilities", RobotProbabilities, queue_size = 10)
        self.temp_out = rospy.Publisher("/results/temperature_data", Float32, queue_size = 10)
        self.tex_out = rospy.Publisher("/results/texture_data", String, queue_size = 10)
        self.sim_complete = rospy.Publisher("/map_node/sim_complete", Bool, queue_size = 1)
        
        self.num_rows = len(self.config['pipe_map'])
        self.num_cols = len(self.config['pipe_map'][0])
        self.belief_p = np.full(self.num_rows * self.num_cols, 1./(self.num_rows*self.num_cols))
        self.belief = np.full(self.num_rows * self.num_cols, 1./(self.num_rows*self.num_cols))

        self.move_count = 0 
        self.total_move_count = len(self.config['move_list'])
        self.p_move = self.config['prob_move_correct']

        self.rate = rospy.Rate(1)

        self.loop()
        rospy.rate.sleep()
        rospy.signal_shutdown("simulation complete")

 def __init__(self):
     rospy.init_node("robot_logger")
     self.path_result = rospy.Subscriber(
             "/results/path_list",
             AStarPath,
             self.handle_a_star_algorithm_path
     )
     self.policy_result = rospy.Subscriber(
             "/results/policy_list",
             PolicyList,
             self.handle_mdp_policy_data
     )
     self.q_policy_result = rospy.Subscriber(
             "/results/qlearn_policy_list",
             PolicyList,
             self.handle_qlearn_policy_data
     )
     self.simulation_complete_sub = rospy.Subscriber(
             "/map_node/sim_complete",
             Bool,
             self.handle_shutdown
     )
     self.init_files()
     self.config = read_config()
     self.generate_video = self.config["generate_video"] == 1
     rospy.spin()

    def __init__(self):
        """Read config file and setup ROS things"""
        self.config = read_config()
        self.config["prob_move_correct"] = .75
        rospy.init_node("map_server")
        self.map_data_service = rospy.Service(
                "requestMapData",
                requestMapData,
                self.handle_data_request
        )
        self.move_service = rospy.Service(
                "moveService",
                moveService,
                self.handle_move_request
        )
	self.found_goal_publisher = rospy.Publisher(
		"/found_goal",
		Bool, 
		queue_size = 10
	)
	self.robot_location_publisher = rospy.Publisher(
		"/robot_location",
		RobotLocation, 
		queue_size = 10
	) 
        self.pos = self.initialize_position()
        r.seed(self.config['seed'])
        print "STARTING POSITION: ", self.pos
        rospy.spin()

    def init_ros_attribute(self):
        self.config = read_config() 
        self.start = self.config["start"]
        self.goal = self.config["goal"] 
        self.walls = self.config["walls"]
        self.pits = self.config["pits"]
        self.max_iter = self.config["max_iterations"]
        self.movelists = self.config["move_list"]
        self.wall_reward = self.config["reward_for_hitting_wall"]
        self.goal_reward = self.config["reward_for_reaching_goal"]
        self.pit_reward = self.config["reward_for_falling_in_pit"]
        self.step_reward = self.config["reward_for_each_step"]
        self.discount= self.config["discount_factor"]
        self.prob_fwd= self.config["prob_move_forward"]
        self.prob_back= self.config["prob_move_backward"]
        self.prob_left= self.config["prob_move_left"]
        self.prob_right= self.config["prob_move_right"]
        self.threshold = self.config["threshold_difference"]
        self.iteration = self.config["max_iterations"]
        self.map_s = self.config["map_size"]

        self.fwd = [[0 for x in range(self.map_s[1])] for x in range(self.map_s[0])]
        self.back= [[0 for x in range(self.map_s[1])] for x in range(self.map_s[0])]
        self.astar = [[0 for x in range(self.map_s[1])] for x in range(self.map_s[0])]
        self.path = [[[1,1] for x in range(self.map_s[1])] for x in range(self.map_s[0])]

def solve():
    config = read_config()
    
    row_size = config['map_size'][0]
    col_size = config['map_size'][1]

    forward_cost_map = np.zeros((row_size, col_size))
    walls = config['walls']
    pits = config['pits']

    start = config['start']
    goal = config['goal']

    move_list = config['move_list']

    ###############
    # Set up grid #
    ###############
    for wall in walls:
        forward_cost_map[wall[0]][wall[1]] = -1000

    for pit in pits:
        forward_cost_map[pit[0]][pit[1]] = -1000

    for x in range(0, row_size):
        for y in range(0, col_size):
            if forward_cost_map[x][y] == -1000:
                continue
            forward_cost_map[x][y] = abs(goal[0]-x) + abs(goal[1]-y)

    ############
    # Solve A* #
    ############
    move_count = 0
    move_queue = []
    prev_map = [[[-1,-1] for i in xrange(col_size)] for j in xrange(row_size)]
    heapq.heappush(move_queue, (0, 0, start, []))

    while len(move_queue) > 0:
        move_set = heapq.heappop(move_queue)
        backward_cost = move_set[1] + 1
        cur_pos = move_set[2]
        path = deepcopy(move_set[3])
        path.append(cur_pos)
        if(cur_pos == goal):
            return path
        x = cur_pos[0]
        y = cur_pos[1]
        for move in move_list:
            new_x = x + move[0]
            new_y = y + move[1]
            if(new_x < 0 or new_x >= row_size or new_y < 0 or new_y >= col_size):
                continue
            if(forward_cost_map[new_x][new_y] == -1000):
                continue
            cost = forward_cost_map[new_x][new_y] + backward_cost
            prev_map[new_x][new_y] = cur_pos
            heapq.heappush(move_queue, (cost, backward_cost, [new_x, new_y], path))
        
    return [] 

    def __init__(self):
        """Read config file and setup ROS things"""
        self.config = read_config()
        rospy.init_node("robot")

        # result publisher
        astar_publisher = rospy.Publisher(
                "/results/path_list",
                AStarPath,
                queue_size = 10
        )
        mdp_publisher = rospy.Publisher(
                "/results/policy_list",
                PolicyList,
                queue_size = 10
        )
        qlearning_publisher = rospy.Publisher(
                "/results/qlearning_list",
                PolicyList,
                queue_size = 10
        )
        # qvalues_publisher = rospy.Publisher(
        #         "/results/qvalue_list",
        #         QvalueLlist,
        #         queue_size = 10
        # )
        complete_publisher = rospy.Publisher(
                "/map_node/sim_complete",
                Bool,
                queue_size = 10
        )


        rospy.sleep(1)
        policyList = PolicyList()
        mdpRes = mdp()
        # print mdpRes
        policyList.data = reduce(lambda x,y: x+y , mdpRes)
        mdp_publisher.publish(policyList)
        rospy.sleep(1)

        qlearningList = PolicyList()
        qlearningObj = qlearning()
        qlearningRes = qlearningObj.exeqlearning()
        # print qlearningRes
        qlearningList.data = reduce(lambda x,y: x+y , qlearningRes)
        qlearning_publisher.publish(qlearningList)
        rospy.sleep(1)

        for i in range(0, len(mdpRes)):
            print "Row: %d" % i
            print "      MDP",
            print mdpRes[i]
            print "qlearning",
            print qlearningRes[i]
        rospy.sleep(1)

        complete_publisher.publish(True)
        rospy.sleep(2)
        rospy.signal_shutdown("Finish")

 def __init__(self):
     """Read config file and setup ROS things"""
     self.config = read_config()
     rospy.init_node("temperature_sensor")
     self.temperature_requester = rospy.ServiceProxy(
             "requestMapData",
             requestMapData
     )
     self.activation_service = rospy.Subscriber(
             "/temp_sensor/activation",
             Bool,
             self.handle_activation_message
     )
     self.temperature_publisher = rospy.Publisher(
             "/temp_sensor/data",
             temperatureMessage,
             queue_size = 10
     )
     self.temp_dict = {
             'H': 40.0,
             'C': 20.0,
             '-': 25.0
     }
     self.temp_message = temperatureMessage()
     self.sensor_on = False
     random.seed(self.config['seed'])
     self.sensor_loop()

	def __init__(self):
		rospy.init_node("robot2") 
		self.config = read_config()

		self.map_publisher = rospy.Publisher(
                "/results/policy_list",
                PolicyList,
                queue_size = 10
        )

		self.map_subscriber = rospy.Subscriber(
                "/results/policy_list",
                PolicyList,
                self.handle_map_message
        )

		self.robot2_sub = rospy.Subscriber(
				"robot2_turn",
				Bool,
				self.handle_turn
		)

		self.reached_goal_pub = rospy.Publisher(
                "reached_goal",
                Bool,
                queue_size = 10
        )

		self.robot1_activator = rospy.Publisher(
				"robot1_turn",
				Bool,
				queue_size = 10
		)

		rospy.spin()

	def __init__(self):
		self.config = read_config()
		self.cost = 0
		self.is_wall = False
		self.going_back = False
		self.m_size = self.config["map_size"]
		self.c_step = self.config["cost_for_each_step"]
		self.c_wall = self.config["cost_for_hitting_wall"]
		self.c_goal = self.config["cost_for_reaching_goal"]
		self.c_pit = self.config["cost_for_falling_in_pit"]
		self.c_up = self.config["cost_for_up"]
		self.c_down = self.config["cost_for_down"]
		self.c_charge = self.config["cost_for_charge"]
		self.goal = self.config["goal"]
		self.start = self.config["start"]
		self.walls = self.config["walls"]
		self.pits = self.config["pits"]
		self.terrain = self.config["terrain_map"]
		self.charge = self.config["charging_stations"]
		self.fuel = self.config["fuel_capacity"]
		self.path = []
		self.cs_path = []
		self.policies = np.full([self.m_size[0], self.m_size[1]], "EMPTY", dtype=object)
		self.policies[self.goal[0]][self.goal[1]] = "GOAL"
		for wall in self.walls:
			self.policies[wall[0]][wall[1]] = "WALL"
		for pit in self.pits:
			self.policies[pit[0]][pit[1]] = "PIT"
		self.is_cs = False
		self.init_vals()
		self.val_iter(self.goal)

	def __init__(self):
		self.config = read_config()
		self.finished_pub = rospy.Publisher("/map_node/sim_complete", Bool ,queue_size=20)
		self.path_list = rospy.Publisher("/results/path_list", AStarPath ,queue_size=30)
		self.policy_list = rospy.Publisher("/results/policy_list", PolicyList ,queue_size=100)
		rospy.init_node('robot',anonymous=True)
		self.rate = rospy.Rate(1)
		rospy.sleep(1)
		# move_list = self.config["move_list"]
		# map_size = self.config["map_size"]
		# start = self.config["start"]
		# goal = self.config["goal"]
		# walls = self.config["walls"]
		# pits = self.config["pits"]
		# cost = self.config["reward_for_each_step"]
		# a_star = astar(move_list,map_size,start,goal,walls,pits,cost)
		# mdp_policies = mdp(self.config)
		reinforcement = ReinforcementLearning(self.config)
		# # for move in a_star:
		# # 	self.path_list.publish(move)
		# rospy.sleep(1)
		# for policy in mdp_policies:
			# self.policy_list.publish(mdp_policies[len(mdp_policies)-1])
		for i in range(1000):
			self.policy_list.publish(reinforcement.allPolicies[999])
			# rospy.sleep(1)
			
		rospy.sleep(1)
		self.finished_pub.publish(True)
		rospy.sleep(1)
		rospy.signal_shutdown("finished moving")

def load_config_file():
	global pipe_map, json_data, variance, res_pipe_map, prob_move_correct, prob_move_inc, num_rows, num_cols

	json_data = read_config()

	pipe_map = json_data["pipe_map"]
	variance = json_data["temp_noise_std_dev"]

	num_rows = len(pipe_map)
	num_cols = len(pipe_map[0])
	prob_move_correct = json_data["prob_move_correct"]
	prob_move_inc = (1 - prob_move_correct)/4.0
	
	prob_x = 1.0/(num_rows * num_cols)
	res_pipe_map= [[prob_x for x in range(num_cols)] for y in range(num_rows)]

	for idx_r in range(num_rows):
		for idx_c in range(num_cols):
			element = pipe_map[idx_r][idx_c]			
			if element  == "C":
				pipe_map[idx_r][idx_c] = COLD_TEMP
			elif element == "-":
				pipe_map[idx_r][idx_c] = ROOM_TEMP
			elif element == "H":
				pipe_map[idx_r][idx_c] = HOT_TEMP

 def __init__(self):
     rospy.init_node("robot_logger")
     self.base_pose_ground_truth_sub = rospy.Subscriber(
             "base_pose_ground_truth",
             Odometry,
             self.update_ground_truth
     )
     self.particlecloud_sub = rospy.Subscriber(
             "particlecloud",
             PoseArray,
             self.update_particlecloud
     )
     self.json_update = rospy.Subscriber(
             "result_update",
             Bool,
             self.result_update
     )
     self.simulation_complete_sub = rospy.Subscriber(
             "sim_complete",
             Bool,
             self.handle_shutdown
     )
     self.init_files()
     self.config = read_config()
     self.num_particles = self.config['num_particles']
     rospy.spin()

def run_test():
    frameHeight, frameWidth = read_config('frameHeight'), read_config('frameWidth')
    frameHeightFace, frameWidthFace = read_config('frameHeightFace'), read_config('frameWidthFace')
    randomState = 51
    debugMode = read_config('debugMode')
    modelNo = read_config('modelNo')
    miniBatchSize = read_config('miniBatchSize')
    verbose = read_config('verbose')

    # Model Initialization
    cnnModel = Sequential()

    if modelNo == 0:
        cnnModel = CNN_model(frameHeight, frameWidth)
    elif modelNo == 1:
        cnnModel = two_inputs_cnn_rnn_model(frameHeight, frameWidth, frameHeightFace, frameWidthFace)

    testPath = '../test'
    testVideos = glob.glob(os.path.join(testPath, '*.mp4'))
    xTest = []

    resultsDir = '../results'
    if not os.path.isdir(resultsDir):
        print('Creating results dir\n')
        os.mkdir(resultsDir)

    weightsDir = '../weights'
    if not os.path.isdir(weightsDir):
        print('No trained weights found, exiting..')
        return

    weights = glob.glob(os.path.join(weightsDir, '*.h5'))

    for testVideo in testVideos:
        fileBase = os.path.basename(testVideo)
        videoPath = os.path.join(testPath, testVideo)
        facesPath = os.path.join(testPath, fileBase.split('.')[0])
        faces = glob.glob(os.path.join(facesPath, '*.png'))
        numFrames = len(faces)
        videoNP = load_avi.load_avi_into_nparray(videoPath, frameHeight, frameWidth, 0, numFrames)
        facesNP = load_avi.load_face_into_nparray(facesPath, frameHeightFace, frameWidthFace, 0, numFrames)
        if modelNo == 0:
            xTest = videoNP
        elif modelNo == 1:
            xTest = [videoNP, facesNP]

        predictionsAll = np.empty((0,numFrames))
        for weight in weights:
            cnnModel.load_weights(weight)
            predictions = cnnModel.predict(xTest, miniBatchSize, verbose)
            predictionsAll = np.append(predictionsAll, predictions, axis=0)
        predictions = np.mean(predictionsAll, axis=0)
        predictions = np.round(predictions)
        resultFileName = os.path.join(resultsDir, fileBase.split('.')[0] + '.txt')
        resultFile = open(resultFileName, 'w')
        resultFile.write(predictions)
        resultFile.close()

	def __init__(self):

		rospy.init_node('Robot') 
		self.config = read_config()
		self.poseArray = None

		rospy.Subscriber("/map", OccupancyGrid, self.handleMapMessage)

		rospy.Subscriber("/base_scan", LaserScan, self.handleLaserMessage)
		
		self.particlePublisher = rospy.Publisher("/particlecloud", PoseArray, queue_size=10, latch=True)

		self.lMapPublisher = rospy.Publisher("/likelihood_field", OccupancyGrid, queue_size=10, latch=True)
		self.resultUpdatePub = rospy.Publisher("/result_update", Bool, queue_size=10, latch=True)
		
		self.simComplPub = rospy.Publisher("/sim_complete", Bool, queue_size=10)

		self.p_i = 0	
		self.particleArray = []
		self.angleMin = 0
		self.poseArray = PoseArray() 	
		self.poseArray.header.stamp = rospy.Time.now()
		self.poseArray.header.frame_id = 'map'
		self.poseArray.poses = []
		self.increment = 0	
		self.index = 0
		self.first = 0	
		self.laserVals = None	
		self.map = None
		self.newWeights = []	
		self.mAngle = None
		self.mDist = None 
		self.mSteps = None
		self.PzForAllParticles = []
		self.totalPzForAllParticles = []
		
		while self.map == None:
			rospy.sleep(0.1)
			
		self.initializeParticles()
		rospy.sleep(0.1)	

		self.constructLMap()
		rospy.sleep(0.1) #why the heck do I need these?!
		
		with open ("log1.txt", 'a') as infile:
			infile.write("from init robot function")
	
		while self.laserVals == None:
			rospy.sleep(0.1)	

		rospy.sleep(0.1)	
		
		self.moveParticles()

		rospy.sleep(0.1)	

		while not rospy.is_shutdown():
			rospy.sleep(0.5)