def connect_mysql(self):
'''
:return:
'''
config = {
# 'host': '192.168.109.34',
'host': read_config().get_option_of_section('db', 'host'),
'port': 3306,
'user': read_config().get_option_of_section('db', 'user'),
'password': read_config().get_option_of_section('db', 'password'),
'db': read_config().get_option_of_section('db', 'db_name'),
'charset': 'utf8'
}
print(config)
db = pymysql.connect(**config)
cursor = db.cursor() # cursor() 方法获取操作游标
cursor.execute(self.sql_string()) # 执行SQL语句
results = cursor.fetchall() # 获取所有记录列表
db.commit() # 没有设置默认自动提交，需要主动提交，以保存所执行的语句
# 除了查询其他操作都需要保存执行
cursor.close()
db.close() # 关闭数据库连接
return results
def __init__(self):
read_config()
self.config = {
"host": os.environ["HOST"],
"user": os.environ["USER"],
"password": os.environ["PASSWORD"],
"port": os.environ["PORT"],
"db": os.environ["DB"],
}
def __init__(self):
self.config = read_config()
self.walls = State.parseMultiple(Point.parseList(self.config["walls"]))
self.pits = State.parseMultiple(Point.parseList(self.config["pits"]))
self.map_height, self.map_width = self.config["map_size"]
self.goalState = State(Point.parseSingle(self.config["goal"]))
self.startState = State(Point.parseSingle(self.config["start"]))
self.max_iterations = self.config["max_iterations"]
self.threshold_diff = self.config["threshold_difference"]
self.r_step = self.config["reward_for_each_step"]
self.r_wall = self.config["reward_for_hitting_wall"]
self.r_goal = self.config["reward_for_reaching_goal"]
self.r_pit = self.config["reward_for_falling_in_pit"]
self.discount_factor = self.config["discount_factor"]
self.alpha = self.config["learning_rate"]
self.epsilon = self.config["epsilon"]
self.p_forward = self.config["prob_move_forward"]
self.p_backward = self.config["prob_move_backward"]
self.p_left = self.config["prob_move_left"]
self.p_right = self.config["prob_move_right"]
self.controller = self.CONTROLLER_VALUE_ITERATION
controller_choice = self.config["controller"].strip()
if controller_choice == "ValueIteration":
self.controller = self.CONTROLLER_VALUE_ITERATION
elif controller_choice == "QLearner":
self.controller = self.CONTROLLER_Q_LEARNER
elif controller_choice == "ApproxQLearner":
self.controller = self.CONTROLLER_APPROX_Q_LEARNER
self.isMovementUncertain = self.config["uncertain_movement"]
def extract_catparams(cfg_catalog, CFG_DIR, output_coord_frame='fk5'):
print('Reading configuration script: %s' % (CFG_DIR + cfg_catalog))
catalog = read_config(CFG_DIR + cfg_catalog)
frame = catalog['frame'] # Coordinate frame used in catalog
source = catalog['source_name']
print('Loading astrometric paramters of %s' % (source))
ref_epoch = Time(catalog['ref_epoch'], format='mjd')
position = SkyCoord(ra=catalog['ra'] * u.deg,
dec=catalog['dec'] * u.deg,
frame=frame).transform_to(output_coord_frame)
ra = position.ra # Degrees
dec = position.dec # Degrees
pmra_cosdec = catalog['pmra_cosdec'] * u.mas / u.yr # mas/yr
pmdec = catalog['pmdec'] * u.mas / u.yr # mas/yr
parallax = catalog['parallax'] * u.mas # mas
# Errors on astrometric parameters
err_RAcosdec = catalog['err_ra_cosdec'] * u.mas # mas
err_RA = err_RAcosdec / np.cos(dec.rad) # mas
err_DEC = catalog['err_dec'] * u.mas # mas
err_pmra_cosdec = np.nan_to_num(
catalog['err_pmra_cosdec']) * u.mas / u.yr # mas/yr
err_pmdec = np.nan_to_num(catalog['err_pmdec']) * u.mas / u.yr # mas/yr
err_parallax = catalog['err_parallax'] * u.mas # mas
# Correlation coefficients
ra_dec_corr = catalog['ra_dec_corr']
ra_pmracosdec_corr = catalog['ra_pmracosdec_corr']
ra_pmdec_corr = catalog['ra_pmdec_corr']
dec_pmracosdec_corr = catalog['dec_pmracosdec_corr']
dec_pmdec_corr = catalog['dec_pmdec_corr']
pmracosdec_pmdec_corr = catalog['pmracosdec_pmdec_corr']
return position, ref_epoch, ra, dec, pmra_cosdec, pmdec, parallax, err_RAcosdec, err_RA, err_DEC, err_pmra_cosdec, err_pmdec, err_parallax, ra_dec_corr, ra_pmracosdec_corr, ra_pmdec_corr, dec_pmracosdec_corr, dec_pmdec_corr, pmracosdec_pmdec_corr
def mdp(robot):
grid = robot.mapServer.grid
config = read_config()
initialize(config, robot)
while iteration(grid, config, robot):
pass
def __init__(self):
self.config = read_config()
rospy.init_node("robot_controller")
self.temperature_subscriber = rospy.Subscriber(
"/temp_sensor/data", temperatureMessage,
self.handle_incoming_temperature_data)
self.texture_requester = rospy.ServiceProxy(
"requestTexture",
requestTexture,
)
self.move_requester = rospy.ServiceProxy(
"moveService",
moveService,
)
self.temp_activator = rospy.Publisher("/temp_sensor/activation",
Bool,
queue_size=10)
self.temp_res_pub = rospy.Publisher("/results/temperature_data",
Float32,
queue_size=10)
self.tex_res_pub = rospy.Publisher("/results/texture_data",
String,
queue_size=10)
self.prob_res_pub = rospy.Publisher("/results/probabilities",
RobotProbabilities,
queue_size=10)
self.shutdown_pub = rospy.Publisher("/map_node/sim_complete",
Bool,
queue_size=10)
self.initialize_maps()
self.initialize_beliefs()
self.motions = deque(self.config['move_list'])
rospy.sleep(2)
self.simulate()
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 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 __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):
""" This will read the config files and set up the different
listeners and what not
"""
self.config = read_config()
self.buildMap()
self.runMDP()
self.sensor = sensor(self.config)
self.moveDict = self.sensor.moveSensor.moveDict
self.probDict = dict()
self.probDict[0] = self.config["prob_move_forward"]
self.probDict[1] = self.config["prob_move_backward"]
self.probDict[2] = self.config["prob_move_left"]
self.probDict[3] = self.config["prob_move_right"]
self.count = 0
row, col = self.config["map_size"]
for x in xrange(row):
for y in xrange(col):
if not (self.grid[x][y] == "W" or self.grid[x][y] == "P"):
self.count += 1
avg = 1.0 / float(self.count)
self.probGrid = [[
avg if not (cell == "W" or cell == "P") else 0 for cell in line
] for line in self.grid]
util.print_2d_floats(self.probGrid)
self.getToGoal()
def __init__(self):
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): self.json_data = read_config() self.cost = 1 self.avoid_states = [AStar.OBSTACLE, AStar.PIT] self.create_pub() self.create_map() self.astar()
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 setupConfig(self):
config = read_config()
self.config = config
self.sig = config['temp_noise_std_dev']
self.moveList = config['move_list']
self.energy = config['starter_energy']
self.height = self.mapServer.height
self.width = self.mapServer.width
random.seed(len(self.mapServer.vis.robots) + self.config['seed'])
self.pos = self.mapServer.grid.get(random.randrange(self.width),
random.randrange(self.height))
self.pos.robots.append(self)
self.text_probability = config['prob_tex_correct']
self.move_success = config['prob_move_correct'] if config[
'uncertain_motion'] else 1
self.move_fail = (
1 - self.move_success) / (len(config['possible_moves']) -
1.) if config['uncertain_motion'] else 0
init_probability = 1. / (self.height * self.width)
for p in self.grid.getPoints():
p.prob[self] = init_probability
self.updateTemp()
self.updateTexture()
self.possible_moves = [move for move in config['possible_moves']]
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("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):
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 run(self):
'''
This method generates the Screen Recording file.
It specifies the device screen resolution and takes screenshot using PyAutoGUI.
The screenshot is converted to a numpy array.
PyAutoGUI captures the screen in RGB(Red, Green, Blue) form and OpenCV converts it to BGR(Blue, Green, Red) and then writes that in an output file.
The file is saved in ScreenRecordings folder and named by current date and time in .avi format.
:returns: Screen Recording file
'''
resolution = pyautogui.size()
codec = cv2.VideoWriter_fourcc(*"XVID")
filename = "../results/ScreenRecordings/"+read_config()+"_"+datetime.now().strftime('%d-%m-%Y_%H:%M:%S')+".avi"
fps =24.0
out = cv2.VideoWriter(filename, codec, fps, resolution)
while True:
img = pyautogui.screenshot()
frame = np.array(img)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
out.write(frame)
if self.flag==1:
break
cv2.destroyAllWindows()
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 __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 __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):
# 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_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 __init__(self):
rospy.init_node("robot_logger")
self.path_result = rospy.Subscriber(
"/results/path_list",
AStarPath,
self.handle_a_star_algorithm_path
)
self.policy_ql_result = rospy.Subscriber(
"/results/policy_list",
PolicyList,
self.handle_ql_policy_data
)
self.path_result = rospy.Subscriber(
"/results/pathql_list",
PathList,
self.handle_ql_path_data
)
self.policy_mdp_result = rospy.Subscriber(
"/results/policy_mdp_list",
PathList,
self.handle_mdp_path_data
)
self.simulation_complete_sub = rospy.Subscriber(
"/map_node/sim_complete",
Bool,
self.handle_shutdown
)
self.init_files()
#self.iteration_number = 0
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.height = len(self.config['walls_map'])
self.width = len(self.config['walls_map'][0])
self.grid = Grid((self.width, self.height))
self.grid.commands = self.config['possible_moves']
self.robots = []
self.it = None
self.runners = []
self.frozenRunners = []
r.seed(self.config['seed'])
for p in self.grid.getPoints():
x, y = p.x, p.y
p.prob = {}
p.prob_new = {}
p.isWall = (self.config['walls_map'][y][x] == 'W')
p.temp = self.config['pipe_map'][y][x]
p.text = self.config['texture_map'][y][x]
self.vis = Visual(self)
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 __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): 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):
rospy.init_node("robot_logger")
self.temp_results_sub = rospy.Subscriber(
"/results/temperature_data", Float32,
self.handle_incoming_temperature_data)
self.tex_results_sub = rospy.Subscriber(
"/results/texture_data", String, self.handle_incoming_texture_data)
self.prob_results_sub = rospy.Subscriber(
"/results/probabilities", RobotProbabilities,
self.handle_incoming_probabilities)
"""
self.path_result = rospy.Subscriber(
"/results/path_list",
AStarPath,
self.handle_a_star_algorithm_path
)
"""
self.robot_location = rospy.Subscriber("/robot_location",
RobotLocation,
self.handle_robot_location)
self.policy_result = rospy.Subscriber("/results/policy_list",
PolicyList,
self.handle_mdp_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
self.current_robot_location = []
self.robot_location = self.config["starting_pos"]
self.policy_list = []
rospy.spin()
def __init__(self):
self.config = read_config()
self.n = self.config['num_particles']
# self.n = 100
self.seed = self.config['seed']
self.sigma = self.config['laser_sigma_hit']
self.moves = self.config['move_list']
self.move_index = 0
self.width = 0
self.height = 0
self.min_weight = 0
self.particle_array = []
self.true_map = 0
self.likelihood_map = 0
self.latest_scan = None
rospy.init_node("robot")
self.pose_publisher = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10,
latch=True)
self.likelihood_publisher = rospy.Publisher("/likelihood_field",
OccupancyGrid,
queue_size=10,
latch=True)
self.result_publisher = rospy.Publisher("/result_update",
Bool,
queue_size=10)
self.complete_publisher = rospy.Publisher("/sim_complete",
Bool,
queue_size=10)
self.map_subscriber = rospy.Subscriber("/map", OccupancyGrid,
self.get_map)
self.scan_subscriber = rospy.Subscriber("/base_scan", LaserScan,
self.save_scan)
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 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):
self.config = read_config()
rospy.init_node("robot")
self.temperature_publisher = rospy.Publisher("/temp_sensor/activation",
Bool,
queue_size=10)
self.temp_publisher = rospy.Publisher("/results/temperature_data",
Float32,
queue_size=10)
self.text_publisher = rospy.Publisher("/results/texture_data",
String,
queue_size=10)
self.prob_publisher = rospy.Publisher("/results/probabilities",
RobotProbabilities,
queue_size=10)
self.simulation_complete_pub = rospy.Publisher(
"/map_node/sim_complete", Bool, queue_size=10)
self.temperature_service = rospy.Subscriber(
"/temp_sensor/data", temperatureMessage,
self.handle_temperature_message)
self.texture_requester = rospy.ServiceProxy("requestTexture",
requestTexture)
self.move_requester = rospy.ServiceProxy("moveService", moveService)
self.moveStep = 0
self.num_rows = len(self.config['pipe_map'])
self.num_cols = len(self.config['pipe_map'][0])
self.position = [1.0 / (float)(self.num_rows * self.num_cols)
] * (self.num_rows * self.num_cols)
rospy.sleep(1)
self.temperature_publisher.publish(Bool(True))
rospy.spin()
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()
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 __init__ (self): self.config = read_config() self.move_list = self.config['move_list'] self.max_iterations = self.config['max_iterations'] self.threshold_difference = self.config['threshold_difference'] self.step_reward = self.config['reward_for_each_step'] 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.discount_factor = self.config['discount_factor'] self.prob_move_forward = self.config['prob_move_forward'] self.prob_move_backward = self.config['prob_move_backward'] self.prob_move_left = self.config['prob_move_left'] self.prob_move_right = self.config['prob_move_right'] self.start = self.config['start'] self.goal = self.config['goal'] self.walls = self.config['walls'] self.pits = self.config['pits'] self.map_size = self.config['map_size'] self.row = self.map_size[0] self.col = self.map_size[1] self.mdp_pub = rospy.Publisher( "/results/policy_list", PolicyList, queue_size = 10 ) self.grid = [[0 for x in range(self.col)] for y in range(self.row)] self.policy_list = [[0 for x in range(self.col)] for y in range(self.row)]
def __init__(self, state):
config = read_config()
goalLoc = config["goal"]
walls = config["walls"]
pits = config["pits"]
rewardPit = config["reward_for_falling_in_pit"]
rewardGoal = config["reward_for_reaching_goal"]
rewardWall = config["reward_for_hitting_wall"]
self.state = state
self.canAlter = True
self.isWall = False
self.policy = None
self.value = 0
# Left, Right, Up, Down
self.qVals = {"W": 0, "E": 0, "N": 0, "S": 0}
if (state == goalLoc):
self.policy = "GOAL"
self.value = rewardGoal
self.canAlter = False
else:
for pit in pits:
if (state == pit):
self.policy = "PIT"
self.value = rewardPit
self.canAlter = False
for wall in walls:
if (state == wall):
self.policy = "WALL"
self.value = rewardWall
self.canAlter = False
self.isWall = True
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__(self):
self.config = read_config()
rospy.init_node('robot', anonymous=True)
self.astar_publisher = rospy.Publisher('/results/path_list',
AStarPath,
queue_size=1)
self.mdp_publisher = rospy.Publisher('/results/policy_list',
PolicyList,
queue_size=1)
self.complete_publisher = rospy.Publisher('/map_node/sim_complete',
Bool,
queue_size=1)
# Read in input parameters from config file
self.move_list = self.config['move_list']
self.map_size = self.config['map_size']
self.start = self.config['start']
self.goal = self.config['goal']
self.walls = self.config['walls']
self.pits = self.config['pits']
# A*
self.astar_path()
# MDP
self.mdp_list()
# Publish to sim_complete before shutdown for json result files
rospy.sleep(1)
self.complete_publisher.publish(True)
rospy.sleep(1)
rospy.signal_shutdown(self)
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)
def __init__(self, map):
self.location = (0, 0)
self.config = read_config()
self.prob_move_forward = self.config["prob_move_forward"]
self.prob_move_backward = self.config["prob_move_backward"]
self.prob_move_left = self.config["prob_move_left"]
self.prob_move_right = self.config["prob_move_right"]
self.map = deepcopy(map)
def __init__(self, map): self.location = (0,0) self.config = read_config() self.prob_move_forward = self.config["prob_move_forward"] self.prob_move_backward = self.config["prob_move_backward"] self.prob_move_left = self.config["prob_move_left"] self.prob_move_right = self.config["prob_move_right"] self.map = deepcopy(map)
def test1_way(self):
"""Load config and checking data"""
error, graphs, tokens, log, lang = read_config.read_config(
'for_tests/config_6.ini')
self.assertFalse(error)
graph = classes.GraphObject()
graph.append(graphs)
assert graph.len() == 1
def __init__(self):
rospy.init_node("laser_model")
self.config = read_config()
r.seed(self.config['seed'])
self.laser_sigma_hit = self.config['laser_sigma_hit']
rospy.Subscriber('base_scan', LaserScan, self.scan_callback)
self.base_scan_pub = rospy.Publisher('base_scan_with_error', LaserScan, queue_size = 10)
rospy.spin()
def __init__(self):
rospy.init_node("robot")
self.config = read_config()
#r.seed(self.config['seed'])
self.num_particles = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.move_list = self.config['move_list']
self.move_list_length = len(self.move_list)
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_theta = self.config['resample_sigma_angle']
self.handle_map_first_called = 1
self.move_made = 0
self.map_data_sub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.handle_map_data
)
self.base_scan_data = rospy.Subscriber(
"/base_scan",
LaserScan,
self.handle_base_scan_data
)
self.particle_pose_pub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihood_pub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 1,
latch = True
)
self.result_update_pub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.sim_complete_pub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
self.scan_data_avai = 0
self.sensor_loop()
def __init__(self):
rospy.init_node("robot")
self.config = read_config()
self.text_map = self.config["texture_map"]
self.pipe_map = self.config["pipe_map"]
self.move_correct_prob = self.config["prob_move_correct"]
self.text_correct_prob = self.config["prob_tex_correct"]
self.sd = self.config["temp_noise_std_dev"]
self.move_list = self.config["move_list"]
self.prob_array = []
self.output_prob_array = []
self.current_pos = self.config["starting_pos"]
self.init_prob()
#create publisher to active temp_sensor/activation
self.active_pub = rospy.Publisher("/temp_sensor/activation",
Bool,
queue_size=1)
#create a subscriber to recive data from temp_sensor/data
self.temp_sensor_sub = rospy.Subscriber("/temp_sensor/data",
temperatureMessage,
self.handle_temp)
#request map server to make move
self.move = rospy.ServiceProxy("moveService", moveService)
#write temperture data into file
self.write_temp = rospy.Publisher('/results/temperature_data',
Float32,
queue_size=10)
#write texture into file
self.write_texture = rospy.Publisher('/results/texture_data',
String,
queue_size=10)
#write probabilities into file
self.write_prob = rospy.Publisher('/results/probabilities',
RobotProbabilities,
queue_size=10)
'''self.terminator = rospy.Publisher(
'/map_node/sim_complete',
Bool,
queue_size = 1
)'''
#reqeust texture from requesttexture
self.texture_request = rospy.ServiceProxy("requestTexture",
requestTexture)
#final publish call to create output file
self.terminator = rospy.Publisher('/map_node/sim_complete',
Bool,
queue_size=1)
rospy.sleep(1)
self.active_pub.publish(True)
self.prob = RobotProbabilities()
self.temp_data = np.float32(0.0)
self.move_made = 0
self.total_move = len(self.move_list)
self.active_pub.publish(True)
rospy.spin()
def __init__(self):
""" This will read the config files and set up the different
listeners and what not
"""
self.config = read_config()
self.move_list = self.config['move_list']
self.activated = False
self.temp_dict = {'H': 40.0, 'C': 20.0, '-': 25.0}
self.move_num = 0
self.count = 0
self.sent = 0
for line in self.config['pipe_map']:
for cell in line:
self.count += 1
avg = 1.0/float(self.count)
self.probPosition = [[avg for cell in line] for line in self.config['pipe_map']]
self.temp_activation_pub = rospy.Publisher(
"/temp_sensor/activation",
Bool,
queue_size = 10
)
self.temperature_sub = rospy.Subscriber(
"/temp_sensor/data",
temperatureMessage,
self.beliefupdate
)
self.tex_req = rospy.ServiceProxy(
"requestTexture",
requestTexture
)
self.move_maker = rospy.ServiceProxy(
"moveService",
moveService
)
self.prob_pub = rospy.Publisher(
"/results/probabilities",
RobotProbabilities,
queue_size = 10
)
self.temp_pub = rospy.Publisher(
"/results/temperature_data",
Float32,
queue_size = 10
)
self.tex_pub = rospy.Publisher(
"/results/texture_data",
String,
queue_size = 10
)
self.sim_status = rospy.Publisher(
"/map_node/sim_complete",
Bool,
queue_size = 10
)
rospy.init_node("robot")
self.start()
def init_config(self):
self.config = read_config()
self.move_list = self.config["move_list"]
map_size_arr = self.config["map_size"]
self.map_height = map_size_arr[0]
self.map_width = map_size_arr[1]
self.start = self.config["start"]
self.goal = self.config["goal"]
self.walls = self.config["walls"]
self.pits = self.config["pits"]
def __init__(self):
""" This will read the config files and set up the different
listeners and what not
"""
self.config = read_config()
self.robotInit()
rospy.init_node("robot")
self.start()
def calculateUtility(position, move_list, map_size, pits, walls, rewardMap):
# Intitialize Variables
config = read_config()
discount_factor = config["discount_factor"]
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"]
generate_video = config["generate_video"]
# Check if position is a pit or a wall
if(position in pits):
return
if(position in walls):
return
neighbors = getNeighbors(position[0], position[1], move_list, map_size)
forwardReward = 0
forward = []
neighborCount = 0
for i in range(0, len(neighbors)):
neighbor = neighbors[i]
if(neighbor[0] != "-" and rewardMap[neighbor[0]][neighbor[1]] > forwardReward):
forwardReward = rewardMap[neighbor[0]][neighbor[1]]
forward = neighbor
neighborCount = i
if (forward != []):
left, right = leftRightForward(neighborCount, forward)
if (left[0] < 0
or left[1] < 0
or left[0] >= map_size[0]
or left[1] >= map_size[1]):
leftReward = 0
else:
leftReward = rewardMap[left[0]][left[1]]
if (right[0] < 0
or right[1] < 0
or right[0] >= map_size[0]
or right[1] >= map_size[1]):
rightReward = 0
else:
rightReward = rewardMap[right[0]][right[1]]
rewardMap[position[0]][position[1]] = rewardMap[position[0]][position[1]] * discount_factor
fReward = (prob_move_forward*forwardReward)
lReward = (prob_move_left*leftReward)
rReward = (prob_move_right*rightReward)
rewardMap[position[0]][position[1]] += fReward + lReward + rReward
def mdp(): config = read_config() move_list = config["move_list"] mapWidth = config["map_size"][1] mapHeight = config["map_size"][0] obstacles = config["walls"] pits = config["pits"] start = config["start"] goal = config["goal"] iter_limit = config["max_iterations"] threshold = config["threshold_difference"] board = [] for i in range(mapHeight): board.append([]) for j in range(mapWidth): cur = boardPos(0, "E") if [i, j] in obstacles: cur.value = config["reward_for_hitting_wall"] cur.policy = "WALL" elif [i, j] in pits: cur.value = config["reward_for_falling_in_pit"] cur.policy = "PIT" elif [i, j] == goal: cur.value = config["reward_for_reaching_goal"] cur.policy = "GOAL" board[i].append(cur) #print len(result), len(result[0]) policy = [] for i in range(mapHeight): for j in range(mapWidth): policy.append(board[i][j].policy) policy_publisher.publish(PolicyList(policy)) rospy.sleep(1) iter_count = 0 while iter_count < iter_limit: board, stop = update_board(board, config) policy = [] for i in range(mapHeight): for j in range(mapWidth): policy.append(board[i][j].policy) policy_publisher.publish(PolicyList(policy)) if stop: break iter_count += 1 for i in range(mapHeight): for j in range(mapWidth): print i, j, board[i][j]
def __init__(self):
rospy.init_node('robot')
self.config = read_config()
r.seed(self.config["seed"])
self.rate = rospy.Rate(1)
self.num_moves = 0
self.moved = False
self.bootstrap()
rospy.spin()
def __init__(self):
# initialize robot node
rospy.init_node("robot")
# parse config file
self.config = read_config()
self.goal = self.config['goal']
self.threshold = self.config['threshold']
# initialize temperature and texture fields
self.temperature = 0.0
self.texture = ''
self.position = [0,0]
self.move = ''
# initialize robot's belief about position
initial_belief = 1/(float((len(self.config['pipe_map']) * len(self.config['pipe_map'][0]))) - float(len(self.config['walls'])))
self.prior_belief = [[initial_belief for x in range(len(self.config['pipe_map'][y]))] for y in range(len(self.config['pipe_map']))]
for x, row in enumerate(self.prior_belief):
for y, col in enumerate(row):
if [x,y] in self.config['walls']:
col = 0.0
#calculate MDP here
self.policy = mdp(self.config)
self.move_instruction = {
'N': [-1,0],
'W': [0,-1],
'S': [1,0],
'E': [0,1],
'WALL': [2,2],
'GOAL': [0,0]
}
# Initialize subscribers, publishers, and services
self.init_ros_things()
# Publish bool message to temp sensor's activation topic
bool_msg = Bool()
bool_msg.data = True
self.temperature_activation.publish(bool_msg)
# Publish the results of MDP
policy_list = [x for y in self.policy for x in y]
self.policy_publisher.publish(policy_list)
rospy.sleep(1)
rospy.spin()
rospy.signal_shutdown(self)
def robot():
global height
global length
global localMap, ProbMap, cleanMap, textureMap, numR, numS
numR=0
numS=0
for j in range (0, height):
row =[]
row2=[]
for k in range (0, length):
if textureMap[j][k]=='R':
numR+=1
else:
numS+=1
row.append(1.0/(height*length))
row2.append(0)
localMap.append(row)
ProbMap.append(row2)
#print ProbMap
#start=read_config()["starting_pos"]
# global posX, posY
# posX=start[0]
# posY=start[1]
# localMap[start[0]][start[1]]=1
pub = rospy.Publisher('/temp_sensor/activation', Bool, queue_size=10)
tempMess=rospy.Subscriber("/temp_sensor/data", temperatureMessage,
callBack)
posSub=rospy.Subscriber('position', positionMessage, callback1)
rospy.init_node('robot.py', anonymous=True)
rate=rospy.Rate(1)
while not rospy.is_shutdown():
pub.publish(True)
rate.sleep()
#rate = rospy.Rate(10) #10hz
#print "move to pos: "
maxloop=len(read_config()["move_list"])
#while i < maxloop :
rospy.spin();
