def __init__(self, conf):
logger = logging.getLogger('SimulationMultigoal')
logger.info('Initializing simulation')
self.conf = conf
# for readability
self.pop_size = conf.pop_size
self.map = conf.map
self.robot = conf.robot
self.max_distance = math.sqrt((self.map.plane.shape[0]**2) +
(self.map.plane.shape[1]**2))
self.s_to_t_distance = utils.point_distance(
conf.map.start_point[0, 0, :].tolist(),
conf.map.end_point[0, 0, :].tolist())
self.fitnesses = None
self.net_ids = None
self.robot_angles = None
self.sensor_angles = None
self.robot_positions = None
self.robot_bodies = None
self.sensor_lines = None
self.angle_errors = None
self.normalized_angle_errors = None
self.target_distances = None
self.normalized_target_distances = None
self.max_distance_from_start = None
self.nets = None
def update(self, _origin):
"""Update the laser's position."""
self.origin = _origin
self.angle = self.find_angle(_origin, self.destination)
self.length = utils.point_distance(_origin[0], self.destination[0],
_origin[1], self.destination[1])
self.polar = (self.length, self.angle)
def __init__(self, _p_screen, _p_world):
pygame.sprite.Sprite.__init__(self)
self.screen = _p_screen
self.world = _p_world
self.image = pygame.image.load("robot.png")
self.robot_size = 50
self.image = pygame.transform.smoothscale(self.image,
(self.robot_size, self.robot_size))
self.image = pygame.transform.rotate(self.image, 90)
self.image_size = self.image.get_size()
self.og_image = self.image.copy()
self.rect = self.image.get_rect()
self.x_pos = float(self.screen.get_size()[0] / 2)
self.y_pos = float(self.screen.get_size()[1] / 2)
self.angle = 0
self.rect.center = (self.x_pos, self.y_pos)
self.hitbox = pygame.Rect(self.x_pos - (self.image_size[0] / 2),
self.y_pos - (self.image_size[1] / 2),
self.image_size[0] + 2,
self.image_size[1] + 2)
self.mask = pygame.mask.from_surface(self.image)
self.draw_lidar = True
# Lidar setup
self.sample_rate = 5 # Hz
self.lidar_state = 0
self.sample_count = 32
self.angle_ref = []
self.new_sample = True
self.initial_laser_length = int(utils.point_distance(self.screen.get_width(), 0,
self.screen.get_height(), 0))
def __read_sensors(self):
readings = np.zeros((len(self.sensor_lines[0]), len(self.sensor_lines))) + self.robot.sensor_len
for robot in range(0, len(self.sensor_lines)):
for sensor in range(0, len(self.sensor_lines[robot])):
for line_point in self.sensor_lines[robot][sensor]:
if self.sim_map.plane[line_point[0], line_point[1]] != 1: # 1 means free
# calc distance from start of the sensor line to point of contact
readings[sensor, robot] = utils.point_distance(self.sensor_lines[robot][sensor][0], line_point)
break
readings = self.__normalize_sensor_readings(readings)
return readings
def is_at_position(self, position):
return utils.point_distance(position,
self.get_position()) <= self.radius
def is_at_position(self, position):
return utils.point_distance(position, self.get_position()) <= self.radius
def world_editor(self, _mouse_click, _pos):
"""Draw onto the world grid if mouse is down and draw the current world grid."""
def world_editor_button_hover(_bh_pos):
"""Return true if the position is within any of the world editor buttons."""
_return = np.array([
self.we_clear_btn.hover_point(_bh_pos[0], _bh_pos[1]),
self.we_done_btn.hover_point(_bh_pos[0], _bh_pos[1]),
self.we_mode_btn.hover_point(_bh_pos[0], _bh_pos[1])
])
return _return.any()
def world_editor_centre_hover(_ch_pos):
"""Return true if the position is within where the robot will spawn."""
_hor_cen = self.screen.get_width() / 2
_vert_cen = self.screen.get_height() / 2
_robot_size = self.robot.robot.robot_size
_return = np.array([
_ch_pos[0] > _hor_cen - _robot_size,
_ch_pos[0] < _hor_cen + _robot_size,
_ch_pos[1] < _vert_cen + _robot_size,
_ch_pos[1] > _vert_cen - _robot_size
])
return _return.all()
def pos_to_grid(_pos):
"""Converts game space coordinates to world map grid coordinates."""
return int(_pos / self.world.size)
if _mouse_click:
if self.world.world_type == "Occupancy Grid" or not self.we_draw_mode:
# If in Occupancy Grid mode, find the distance between the last known mouse
# position and find the points in a line between them
if self.last_mouse_pos != None:
_last_point_dis = utils.point_distance(
self.last_mouse_pos[0], _pos[0], _pos[1],
self.last_mouse_pos[1])
else:
_last_point_dis = 0
# If clicking on a button don't draw anything
if (_last_point_dis < 8 and world_editor_button_hover(_pos)
) or _last_point_dis == 0:
_line = []
else:
_line = utils.line_between(self.last_mouse_pos[0],
self.last_mouse_pos[1], _pos[0],
_pos[1])
# Write to the grid map all the points on the line if not in the spawn space
for _point in _line:
if not world_editor_centre_hover(_point):
self.world.write_to_map(self.we_draw_mode,
pos_to_grid(_point[0]),
pos_to_grid(_point[1]))
self.last_mouse_pos = _pos
elif self.world.world_type == "Landmarks":
# If in landmark mode, only place one wall per click
if self.we_raise_click:
if not world_editor_centre_hover(_pos):
self.world.write_to_map(self.we_draw_mode,
pos_to_grid(_pos[0]),
pos_to_grid(_pos[1]))
self.we_raise_click = False
for i in range(len(self.world.grid)):
for j in range(len(self.world.grid[0])):
if self.world.grid[i][j]:
pygame.draw.rect(
self.screen, (0, 0, 0),
pygame.Rect((j * self.world.size, i * self.world.size),
(self.world.size, self.world.size)))
def collision_detector(self):
"""Finds if the robot is colliding and the associated side.
This function uses sprites to determine all of the objects the robot is colliding with,
then finds the closest wall to determine which side of the robot is colliding. To solve for
cases where the robot is colliding with two walls simultaneously, the function utilises
recursion to find the second closest wall.
"""
_collision_list = pygame.sprite.spritecollide(self.robot,
self.world.wall_list,
False,
pygame.sprite.collide_mask)
if len(_collision_list) > 0:
# Find the closest colliding wall
_closest_distance = self.robot.initial_laser_length
_closest_wall = None
for _wall in _collision_list:
cur_distance = utils.point_distance(self.robot.x_pos,
_wall.rect.center[0],
self.robot.y_pos,
_wall.rect.center[1])
if cur_distance < _closest_distance:
s_closest_wall = _closest_wall
_closest_wall = _wall
_closest_distance = cur_distance
# If performing recursion, find the second closest wall
if self.recursion_depth > 0 and not s_closest_wall is None:
_closest_wall = s_closest_wall
_wall = _closest_wall
# Find which side of the robot is closest to the closest wall
_sides = [self.robot.hitbox.midtop, self.robot.hitbox.midright,
self.robot.hitbox.midbottom, self.robot.hitbox.midleft]
_closest_side = -1
_closest_side_distance = self.robot.initial_laser_length
for _i, _side in enumerate(_sides):
distance = utils.point_distance(_side[0],
_wall.rect.center[0],
_side[1],
_wall.rect.center[1])
if distance < _closest_side_distance:
_closest_side_distance = distance
_closest_side = _i
_to_return = None
if _closest_side == 0:
_to_return = "TOP"
if _closest_side == 1:
_to_return = "RIGHT"
if _closest_side == 2:
_to_return = "BOTTOM"
if _closest_side == 3:
_to_return = "LEFT"
# If the robot is already colliding with a wall, collide the second closest wall
if len(self.collision_list) > 0:
if _to_return == self.collision_list[len(self.collision_list) - 1]:
if self.recursion_depth <= 1:
self.recursion_depth += 1
return self.collision_detector()
self.recursion_depth = 0
return _to_return
return None
def lidar(self):
"""Performs all calculations for laser range finding and handles the drawing of lasers.
This function uses sprites to determine all of the objects each laser around the robot is
colliding with, then finds the closest wall. It then finds the closest point on that wall
to the robot.
"""
# TODO: Fix flickering on some diagonal lasers
# TODO: Make lasers that don't find a result return max length instead of previous result
_iterations_per_frame = int(
self.sample_count / (30 / self.sample_rate))
_slice_from = self.lidar_state * _iterations_per_frame
if self.lidar_state == (30 // self.sample_rate) - 2:
# Ensure final slice gets the remainder
_slice_to = self.sample_count
else:
_slice_to = _slice_from + _iterations_per_frame
# Update the position of each of the laser sprites in self.lasers
_lidar = pygame.math.Vector2()
_lidar.xy = (self.x_pos, self.y_pos)
for _sprite in self.lasers.sprites()[_slice_from:_slice_to]:
_sprite.origin = _lidar
_sprite.update()
# Check wall collisions in quadrants
_quad_list = [[[0, 90], operator.ge, operator.ge],
[[90, 181], operator.lt, operator.ge],
[[-90, 0], operator.ge, operator.lt],
[[-181, -90], operator.lt, operator.lt]]
_collision_list = {}
_pixel_buffer = self.world.size * 2
for _quad in _quad_list:
_quad_lasers = pygame.sprite.Group()
_quad_walls = pygame.sprite.Group()
for _laser in self.lasers.sprites()[_slice_from:_slice_to]:
_cur_angle = int(_laser.angle.as_polar()[1])
if _cur_angle >= _quad[0][0] and _cur_angle < _quad[0][1]:
_quad_lasers.add(_laser)
for _wall in self.world.wall_list:
_cur_pos = _wall.rect.center
if _quad[1] == operator.ge:
_x_buf = self.x_pos - _pixel_buffer
else:
_x_buf = self.x_pos + _pixel_buffer
if _quad[2] == operator.ge:
_y_buf = self.y_pos - _pixel_buffer
else:
_y_buf = self.y_pos + _pixel_buffer
if _quad[1](_cur_pos[0], _x_buf):
if _quad[2](_cur_pos[1], _y_buf):
_quad_walls.add(_wall)
_collision_list.update(pygame.sprite.groupcollide(_quad_lasers,
_quad_walls,
False,
False,
pygame.sprite.collide_mask))
if _collision_list:
for _laser in _collision_list:
# For each laser, find the closest wall to the robot it is colliding with
_closest_wall = None
_closest_distance = self.initial_laser_length
for _wall in _collision_list[_laser]:
cur_distance = utils.point_distance(self.x_pos,
_wall.rect.center[0],
self.y_pos,
_wall.rect.center[1])
if cur_distance < _closest_distance:
_closest_wall = _wall
_closest_distance = cur_distance
# Find the closest point on the closest wall to the robot
_current_pos = pygame.math.Vector2()
_current_pos.update(self.x_pos, self.y_pos)
_heading = _laser.angle
_direction = _heading.normalize()
_closest_point = [self.initial_laser_length,
self.initial_laser_length]
for _ in range(self.initial_laser_length):
_current_pos += _direction
if _closest_wall.rect.collidepoint(_current_pos):
_r = np.sqrt(np.square(self.x_pos - _current_pos.x)
+ np.square(self.y_pos - _current_pos.y))
_theta = np.arctan2(-(self.y_pos - _current_pos.y), -
(self.x_pos - _current_pos.x))
_closest_point = [_r, _theta]
break
# Write resulting point to the point cloud
if not _closest_point == [self.initial_laser_length, self.initial_laser_length]:
_cur_angle = (round(_heading.as_polar()[1]) + 450) % 360
try:
self.point_cloud[self.angle_ref.index(
_cur_angle)] = _closest_point
except ValueError:
pass
if self.lidar_state == (30 // self.sample_rate) - 1:
self.new_sample = True
self.lidar_state = 0
else:
self.lidar_state += 1
