def update_env(self, in_obs_pos, in_obs_grid):
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(in_obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
self.ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
# print "after removing:", tmp_order
# if tmp_order[0] == 'T':
# print "environment setting OK"
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(in_obs_grid)
self.obs_re_pos = copy.deepcopy(in_obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
def get_c_ore(self, in_can_info):
# print "input", in_can_info
# print in_can_info[0].pos
t_c_order = []
for ci in range(len(in_can_info)):
tm_tar_pos = in_can_info[ci].pos
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.append(self.tar_pos)
obs_r = []
for i in tm_obs_pos:
obs_r.append(0.035)
ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
while 1:
tm_tar_pos = in_can_info[ci].pos
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.append(self.tar_pos)
obs_r = []
for i in tm_obs_pos:
obs_r.append(0.035)
for i in ore_order:
if i != 'T':
tm_obs_pos[i] = [4.0, 0.0]
tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
# print "after removing:", tmp_order
if tmp_order[0] != 'T':
# print "not ok"
t_c_order.append([])
break
# if tmp_order[0] == -1:
# print "no path"
# t_c_order.append([])
# break
# if len(ore_order) > len(self.obs_pos):
# print "no path"
# t_c_order.append([])
# break
# print "tricky environment for c_ore so extend", ore_order
# ore_order.pop()
# print "delete target", ore_order
# ore_order.extend(tmp_order)
# print "to", ore_order
else:
if tmp_order[0] == 'T':
# print "ok", ore_order
ore_order.pop()
if len(tm_obs_pos) in ore_order:
# print "\n\nThere is target!! warning!!!\n\n"
t_c_order.append([])
else:
t_c_order.append(ore_order)
break
return t_c_order
def get_c_ore(self, in_can_info):
# print "input", in_can_info
# print in_can_info[0].pos
t_c_order = []
for ci in range(len(in_can_info)):
tm_tar_pos = in_can_info[ci].pos
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.append(self.tar_pos)
# tm_obs_pos.extend(self.obs_wall)
tm_ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(tm_ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
ore_order = NG_ore(
tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos,
self.ws_zero,
[self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# ore_order = TM_noplot(tm_ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
while 1:
tm_tar_pos = in_can_info[ci].pos
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.append(self.tar_pos)
for i in ore_order:
if i != 'T':
tm_obs_pos[i] = [3.0, 3.0]
tmp_order = NG_ore(
tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r,
self.rob_pos, self.ws_zero,
[self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
# print "after removing:", tmp_order
if tmp_order[0] != 'T':
print "tricky environment for c_ore so extend", ore_order
ore_order.pop()
print "delete target", ore_order
ore_order.extend(tmp_order)
print "to", ore_order
else:
if tmp_order[0] == 'T':
print "ok", ore_order
break
ore_order.pop()
if len(tm_obs_pos) in ore_order:
print "\n\nThere is target!! warning!!!\n\n"
t_c_order = []
else:
t_c_order.append(ore_order)
return t_c_order
def set_env(self, obstacle_name, obstacle_info, target_name, target_info):
self.obs_r = []
self.obs_pos = []
self.obs_ori = []
self.obs_grid = []
self.tar_pos = [round(target_info[0][0][2], 2), round(-target_info[0][0][1], 2)]
self.tar_grid = [int(round((target_info[0][0][2] - self.ws_zero[0]) * 100)), int(round((-target_info[0][0][1] - self.ws_zero[1]) * 100))]
self.tar_r = round(target_info[0][2][1] * 0.5, 2)
print "number of obstacles", len(obstacle_info)
for i in range(len(obstacle_info)):
self.obs_pos.append([round(obstacle_info[i][0][2], 2), round(-obstacle_info[i][0][1], 2)])
self.obs_grid.append([int(round((obstacle_info[i][0][2] - self.ws_zero[0])*100)), int(round((-obstacle_info[i][0][1] - self.ws_zero[1])*100))])
self.obs_r.append(round(obstacle_info[i][2][1] * 0.5, 2))
self.obs_ori.append([0.0, 0.0, 0.0])
self.object_z = obstacle_info[0][0][0]
print "obstacles", self.obs_pos
print "target_vfh", self.tar_pos
print "target_rviz", target_info[0]
print "robot pos", self.rob_pos
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
self.ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(self.obs_grid)
self.obs_re_pos = copy.deepcopy(self.obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
def get_c_ore(self, in_can_info):
# print "input", in_can_info
# print in_can_info[0].pos
t_c_order = []
for ci in range(len(in_can_info)):
tm_tar_pos = in_can_info[ci].pos
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.append(self.tar_pos)
# tm_obs_pos.extend(self.obs_wall)
tm_ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(tm_ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# ore_order = TM_noplot(tm_ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
ore_order.pop()
t_c_order.append(ore_order)
return t_c_order
def get_can_info(self, in_can_info, in_obs_pos, in_obs_re_pos, in_ore_order, in_tar_pos):
tmp_can_info = []
for i in range(len(in_ore_order)):
tmp_can_info.append(copy.deepcopy(in_can_info))
tmp_obs_pos = copy.deepcopy(in_obs_pos)
tmp_obs_re_pos = copy.deepcopy(in_obs_re_pos)
tmp_ore_order = copy.deepcopy(in_ore_order)
tmp_tar_pos = copy.deepcopy(in_tar_pos)
# print("\nCheck if candidate blocks the target")
for step_i in range(len(tmp_ore_order)):
for i in range(len(tmp_can_info[step_i])):
vfh_obs_pos = copy.deepcopy(tmp_obs_re_pos)
vfh_obs_pos.append(tmp_can_info[step_i][i].pos)
vfh_obs_pos.extend(self.obs_wall)
vfh_tar_pos = copy.deepcopy(tmp_tar_pos)
ob = len(vfh_obs_pos)
vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
# vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.obs_r, self.tar_r)
if vfh[3] == 0:
tmp_can_info[step_i][i].BT = 1 # BT == 1 : The candidate blocks the target.
else: # BT == 0 : The candidate does not block the target.
tmp_can_info[step_i][i].BT = 0
# print("\nCheck if the candidate is accessible.")
for i in range(len(tmp_can_info[step_i])):
vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos)
vfh_obs_pos = copy.deepcopy(tmp_obs_pos)
for si in range(step_i+1):
# print "\nstep", si, "\nbefore", vfh_obs_pos
vfh_obs_pos.remove(tmp_obs_pos[tmp_ore_order[si]])
# print "after", vfh_obs_pos
vfh_obs_pos.append(tmp_tar_pos)
vfh_obs_pos.extend(self.obs_wall)
ob = len(vfh_obs_pos)
vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
if vfh[3] == 0: # A == 1 : The candidate is accessible.
tmp_can_info[step_i][i].A = 0 # A == 0 : The candidate is not accessible.
else: #
tmp_can_info[step_i][i].A = 1
# print("\nCheck the candidate ORC.")
for i in range(len(tmp_can_info[step_i])):
vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos)
vfh_obs_pos = copy.deepcopy(tmp_obs_pos)
vfh_obs_pos.append(tmp_tar_pos)
vfh_obs_pos.extend(self.obs_wall)
ob = len(vfh_obs_pos)
vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
if vfh[3] == 0: # A == 1 : The candidate is accessible.
tmp_can_info[step_i][i].A = 0 # A == 0 : The candidate is not accessible.
tm_tar_pos = copy.deepcopy(vfh_tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(tmp_obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for obs_ori_i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
ore_order.pop() # The last order is always the target so we need to pop the last element.
tmp_can_info[step_i][i].ORC = ore_order
else: #
tmp_can_info[step_i][i].A = 1
return tmp_can_info
def get_env(self, obs_r, tar_r, min_ore):
while 1:
self.obs_grid = []
grid_tmp = copy.deepcopy(self.grid_act)
self.obs_r = obs_r
self.tar_r = tar_r
for ri in self.obs_r:
grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(grid_tmp, ri, 2)
self.obs_grid.append(obs_center_tmp)
grid_tmp, tar_tmp = CUF.place_circle_object_ig(grid_tmp, self.tar_r, 4)
self.tar_grid = copy.deepcopy(tar_tmp)
self.obs_pos = []
for i in self.obs_grid:
xi, yi = i
self.obs_pos.append([round(xi * self.GRID_SIZE + self.ws_zero[0], 2), round(yi * self.GRID_SIZE + self.ws_zero[1], 2)])
self.tar_pos = [round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2), round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)] # target object!
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
# ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
# TM_noplot()
if len(ore_order) > min_ore:
# print"before rearrangemet: ", ore_order
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
# ob = len(tm_obs_pos)
self.ore_order = ore_order
# self.ore_order = TM_plot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
# print "after removing:", tmp_order
if tmp_order[0] == 'T':
# print"environment setting OK"
break
# else:
# print "SHORT...min:", min_ore, "ours:", len(ore_order)
'''
with out additional rearrangement
'''
if len(self.ore_order) > min_ore:
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(self.obs_grid)
self.obs_re_pos = copy.deepcopy(self.obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
def get_env_case1(self):
while 1:
obs_r = [0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035]
tar_r = 0.035
min_ore = 2
self.obs_grid = []
grid_tmp = copy.deepcopy(self.grid_act)
self.obs_r = obs_r
self.tar_r = tar_r
self.obs_grid = [[10, 70], [10, 50], [10, 10],
[20, 70], [20, 50], [19, 31], [24, 25], [20, 10]]
self.tar_grid = [30, 30]
# self.grid_ori = copy.deepcopy(self.grid_act)
# for i in range(len(self.obs_r)):
# print "obs", i, round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2)
# self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
# self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
# break
# for ri in self.obs_r:
# grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(grid_tmp, ri, 2)
# self.obs_grid.append(obs_center_tmp)
# grid_tmp, tar_tmp = CUF.place_circle_object_ig(grid_tmp, self.tar_r, 4)
# self.tar_grid = copy.deepcopy(tar_tmp)
self.obs_pos = []
for i in self.obs_grid:
xi, yi = i
self.obs_pos.append([round(xi * self.GRID_SIZE + self.ws_zero[0], 2), round(yi * self.GRID_SIZE + self.ws_zero[1], 2)])
self.tar_pos = [round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2), round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)] # target object!
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
# ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
if len(ore_order) > min_ore:
# print"environment setting OK"
# print"target", self.tar_pos
# print"obstacles", self.obs_pos
# print"remove", ore_order, "th obstacles"
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
# self.ore_order = ore_order
self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_ore_pos.append(self.obs_pos[i])
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
# tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
if len(tmp_order) == 1:
print"environment setting OK"
break
'''
# with out additional rearrangement
'''
if len(self.ore_order) > min_ore:
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(self.obs_grid)
self.obs_re_pos = copy.deepcopy(self.obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
