def event(event):
if event.type == pygame.QUIT:
global running
running = False
# key control
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE: #regen map
r.world.worldGen()
r.world.randpoints()
r.renderTick()
elif event.key == pygame.K_RETURN:
r.renderTick()
p.findpath()
#r.renderTick()
r.world.worldGen()
r.world.randpoints()
def path_possible(self, target):
''' checks whether a path is possible
args:
nprray[2]: target - target coordinates
return:
True if possible, false if not
'''
current_pos = self.current_position()
current_pos = (int(current_pos[0]), int(current_pos[1]))
#obstacle_grid = args['obstacle_grid']
obstacle_grid = self.driving_grid
reduced_obstacle_grid = self.reduced_driving_grid
rr, cc = ellipse(target[0],
target[1],
9,
9,
shape=self.driving_grid.shape)
for i in range(len(rr)):
obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
reduced_obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
rad = self.current_angle()
robot_state = np.array([current_pos[0], current_pos[1], rad])
robot_extent_global = np.array([[0, 0], [0, 0], [0, 0], [0, 0]])
for i in range(len(self.extent)):
robot_extent_global[i] = np.rint(
(transform_local_coords(robot_state, self.extent[i])))
robot_extent_global = np.transpose(robot_extent_global)
rr, cc = polygon(robot_extent_global[0], robot_extent_global[1])
for i in range(len(rr)):
obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
reduced_obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
other_bot = convert_to_grid_coords(
self.robot_data[(self.robot_id + 1) % 2][1:3])
rr, cc = ellipse(int(other_bot[0]),
int(other_bot[1]),
12,
12,
shape=self.driving_grid.shape)
for i in range(len(rr)):
obstacle_grid[bound(rr[i]), bound(cc[i])] = 1
reduced_obstacle_grid[bound(rr[i]), bound(cc[i])] = 1
#obstacle_grid[rr, cc] = 0
#reduced_obstacle_grid[rr, cc] = 0
#print(obstacle_grid[args['target'][0], args['target'][1]])
path_temp = findpath(current_pos, target, obstacle_grid)
if path_temp:
return True
else:
return False
def smart_path(self, obstacle_grid, target): current_pos = self.current_position() current_pos = (int(current_pos[0]), int(current_pos[1])) path_temp = findpath(current_pos, target, obstacle_grid) if path_temp: if len(path_temp) > 1: path = bspline(np.asarray(path_temp), int(np.sqrt((current_pos[0] - target[0])**2 + (current_pos[1] - target[1])**2)/2), 9) return path else: return np.array(path_temp) else: return np.array([None])
def smart_path(self, obstacle_grid, target):
''' generates the A* path to a target
args:
nparray: obstacle_grid
nparray: target - target location
returns:
nparray: path - returns None if there is no path
'''
current_pos = self.current_position()
current_pos = (int(current_pos[0]), int(current_pos[1]))
path_temp = findpath(current_pos, target, obstacle_grid)
if path_temp:
if len(path_temp) > 1:
path = bspline(
np.asarray(path_temp),
int(
np.sqrt((current_pos[0] - target[0])**2 +
(current_pos[1] - target[1])**2) / 2), 9)
return path
else:
return np.array(path_temp)
else:
return np.array([None])
final_grid[rr, cc] = 0
rr, cc = rectangle((1, 65), (13, 78), shape=final_grid.shape)
final_grid[rr, cc] = 0
final_grid = pad_grid(final_grid, 1)
final_grid = block_padding(blocks, final_grid)
current_pos0 = convert_to_grid_coords(robot_data[0][1:3])
current_pos0 = (int(current_pos0[0]), int(current_pos0[1]))
rr, cc = ellipse(int(blocks[block_i][0]), int(blocks[block_i][1]),
9, 9)
final_grid[rr, cc] = 0
path_temp = findpath(
current_pos0,
(int(blocks[block_i][0]), int(blocks[block_i][1])), final_grid)
if path_temp:
path0 = path_temp
#print(np.asarray(path0))
path0 = bspline(
np.asarray(path_temp),
int(
np.sqrt((current_pos0[0] - blocks[block_i][0])**2 +
(current_pos0[1] - blocks[block_i][1])**2) /
2), 9)
if path0.any():
if len(path0) > 5:
set_robot_state(
def findpath(self, start, end):
return pathfinder.findpath(self.generatepassablemap(), start, end)
def findpath(self, start, end):
return pathfinder.findpath(self.generatepassablemap(), start, end)
def go_to_target(self, args):
if "speed" in args:
speed = args['speed']
else:
speed = 1.5
if 'look_at' in args:
look_at = args['look_at']
else:
look_at = True
current_pos = self.current_position()
current_pos = (int(current_pos[0]), int(current_pos[1]))
#obstacle_grid = args['obstacle_grid']
obstacle_grid = self.driving_grid
reduced_obstacle_grid = self.reduced_driving_grid
if args['block'] == True:
rr, cc = ellipse(args['target'][0],
args['target'][1],
9,
9,
shape=self.driving_grid.shape)
for i in range(len(rr)):
obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
reduced_obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
if args['empty'] == True:
rad = blue_bot.current_angle()
robot_state = np.array([current_pos[0], current_pos[1], rad])
robot_extent_global = np.array([[0, 0], [0, 0], [0, 0], [0, 0]])
for i in range(len(robot_extent)):
robot_extent_global[i] = np.rint(
(transform_local_coords(robot_state, robot_extent[i])))
robot_extent_global = np.transpose(robot_extent_global)
rr, cc = polygon(robot_extent_global[0], robot_extent_global[1])
for i in range(len(rr)):
obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
reduced_obstacle_grid[bound(rr[i]), bound(cc[i])] = 0
#obstacle_grid[rr, cc] = 0
#reduced_obstacle_grid[rr, cc] = 0
#print(obstacle_grid[args['target'][0], args['target'][1]])
path_temp = findpath(current_pos, args['target'], obstacle_grid)
#print(path_temp)
if path_temp:
if len(path_temp) > 1:
path = bspline(
np.asarray(path_temp),
int(
np.sqrt((current_pos[0] - args['target'][0])**2 +
(current_pos[1] - args['target'][1])**2) / 2),
9)
self.current_path = path
if np.sign(speed) * len(path) > np.sign(
speed) * args['early_stop']:
set_robot_state(
self.robot_id,
robot_motion(robot_data[self.robot_id][1:4], path[1],
speed, 1), args['grip'])
else:
if look_at:
motion = robot_motion(
self.robot_data[self.robot_id][1:4],
args['target'],
0,
1,
info=True)
set_robot_state(self.robot_id, [
np.sign(motion[2]) * 0.4, -np.sign(motion[2]) * 0.4
], args['grip'])
if np.abs(motion[2]) < 0.1:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
#print(args['target'])
obstacle_count = 0
if len(self.current_path) > 0:
for i in range(
len(self.current_path
) if len(self.current_path) <= 6 else 6):
if obstacle_grid[int(self.current_path[i][0]),
int(self.current_path[i][1])] == 1:
obstacle_count = obstacle_count + 1
print(obstacle_count)
if obstacle_count >= 3:
print('no path possible ... reducing driving grid')
path_temp = findpath(current_pos, args['target'],
reduced_obstacle_grid)
#print(path_temp)
if path_temp:
if len(path_temp) > 1:
path = bspline(
np.asarray(path_temp),
int(
np.sqrt(
(current_pos[0] - args['target'][0])**2 +
(current_pos[1] - args['target'][1])**2) /
2), 9)
self.current_path = path
if np.sign(speed) * len(path) > np.sign(
speed) * args['early_stop']:
set_robot_state(
self.robot_id,
robot_motion(robot_data[self.robot_id][1:4],
path[1], speed, 1), args['grip'])
else:
if look_at:
motion = robot_motion(
self.robot_data[self.robot_id][1:4],
args['target'],
0,
1,
info=True)
set_robot_state(self.robot_id, [
np.sign(motion[2]) * 0.4,
-np.sign(motion[2]) * 0.4
], args['grip'])
if np.abs(motion[2]) < 0.1:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
self.set_state('idle', grip=args['grip'])
self.current_path = []
else:
print('no reduced path possible, state set to: "blocked"')
self.set_state('blocked', grip=args['grip'])
self.current_path = []
else:
print('returning to path')