Merge pull request #36 from rishyfishy/performanceImporvements

func: teleport and collision-fast, needs wall dims

config.py

@@ -63,10 +63,13 @@
          [1,0,2,0,0,1,0,1],
          [1,1,1,1,1,1,0,2]] # Matrix to define the maze walls
 floor_seed = 5489           # Randomization seed for generating correctfloor pattern
+maze_dim_x = len(walls[0])*wall_segment_length
+maze_dim_y = len(walls)*wall_segment_length
+
 
 # Graphics information
 frame_rate = 60             # Target frame rate (Hz)
-ppi = 16                    # Number of on-screen pixels per inch on display
+ppi = 12                    # Number of on-screen pixels per inch on display
 border_pixels = floor_segment_length * ppi  # Size of the border surrounding the maze area
 
 background_color = (43, 122, 120)

devices/ultrasonic.py

@@ -56,7 +56,7 @@ def __init__(self, info: dict):
         self.outline_thickness = info.get('outline_thickness', 0.25)
 
         # Display measurement when simulating
-        self.visible_measurement = info.get('visible_measurement', False)
+        self.visible_measurement = info.get('visible_measurement', True)
         self.visible_measurement_time = info.get('visible_measurement_time', 0.5)    # Measurement time on screen (s)
         self.visible_measurement_buffer = 0
 
@@ -69,7 +69,7 @@ def __init__(self, info: dict):
         self.reading_bounds = [self.min_range, self.max_range]  # Upper and lower bounds for sensor reading
 
         self.rays = self._define_rays() # Define the initial rays, without detecting collisions
-        self.ray_lengths = [self.max_range for item in self.rays]   # The length of the rays
+        self.ray_lengths_squared = [self.max_range**2 for item in self.rays]   # The length of the rays
 
     def _define_rays(self):
         '''Define the rays used to get the ultrasonic distance.'''
@@ -107,45 +107,44 @@ def draw_measurement(self, canvas):
             # Decrement the buffer
             self.visible_measurement_buffer -= 1
 
-
     def simulate(self, value: float, environment: dict):
         '''
         Simulates the performance of an ultrasonic sensor.
 
         Response data format
         [0:7] - Eight byte double
         '''
-        ROBOT = environment.get('ROBOT', False)
-        MAZE = environment.get('MAZE', False)
+        MAZE = environment.get("MAZE", False)
         BLOCK = environment.get('BLOCK', False)
 
         rays = self._define_rays()
-        ray_lengths = [self.max_range for item in rays]
+        ray_lengths_squared = [self.max_range**2 for item in rays]
 
         # Update the measurement display buffer
         self.visible_measurement_buffer = int(self.visible_measurement_time * CONFIG.frame_rate)
 
+        # Check if the sensor is at a height where the block would be seen
+        if self._block_visible(BLOCK):
+            walls_to_check = BLOCK.block_square + MAZE.reduced_walls
+        else:
+            walls_to_check = MAZE.reduced_walls
+
         for ct, ray in enumerate(rays):
-            # Check if the sensor is at a height where the block would be seen
-            if self._block_visible(BLOCK):
-                to_check = [BLOCK.block_square, *MAZE.wall_squares]
-            else:
-                to_check = MAZE.wall_squares
-
-            for square in to_check:
-                for segment_wall in square:
-                    collision_points = utilities.collision(ray, segment_wall)
-                    if not collision_points:
-                        pass
-                    else:
-                        rays[ct][1], ray_lengths[ct] = utilities.closest(self.position_global, collision_points)
+            for wall in walls_to_check:
+                collision_points = utilities.collision(ray, wall)
+                if not collision_points:
+                    pass
+                else:
+                    rays[ct][1], ray_lengths_squared[ct] = utilities.closest_fast(
+                        self.position_global, collision_points
+                    )
 
         # Update stored variables
         self.rays = rays
-        self.ray_lengths = ray_lengths
+        self.ray_lengths_squared = ray_lengths_squared
 
         # Build the value to return
-        output = min(self.ray_lengths)
+        output = math.sqrt(min(self.ray_lengths_squared))
 
         return utilities.add_error(output, self.error_pct, self.reading_bounds)
 

maze.py

@@ -23,6 +23,8 @@
 import pygame
 import shapely as shp
 import config as CONFIG
+import utilities
+
 
 class Maze:
     '''This class represents the maze/environment'''
@@ -32,6 +34,8 @@ def __init__(self):
         self.size_x = 0
 
         self.wall_squares = []
+        self.walls = []
+        self.reduced_walls = []
         self.floor_tiles = []
         self.floor_tile_colors = 0
         self.floor_rect_black = []
@@ -45,8 +49,8 @@ def import_walls(self):
         dim_y = np.size(wall_map, 0)
         dim_x = np.size(wall_map, 1)
 
-        self.size_y = dim_y * CONFIG.wall_segment_length
-        self.size_x = dim_x * CONFIG.wall_segment_length
+        self.size_y = CONFIG.maze_dim_y
+        self.size_x = CONFIG.maze_dim_x
 
         # Outer maze dimensions
         self.wall_squares.append([
@@ -72,23 +76,26 @@ def import_walls(self):
                               for line in square]
                              for square in self.wall_squares]
 
+        # Flattens list of walls, removes unnecessary walls
+        self.walls= [wall for wallsquare in self.wall_squares for wall in wallsquare]
+        self.reduced_walls = utilities.optimize_walls(self.walls)
+
     def draw_walls(self, canvas):
         '''Draws the maze walls onto the screen'''
 
         # Graphics
         THICKNESS = int(CONFIG.wall_thickness * CONFIG.ppi)
         COLOR = CONFIG.wall_color
 
-        for wall in self.wall_squares:
-            for line in wall:
+        for line in self.reduced_walls:
                 start = [scalar * CONFIG.ppi + CONFIG.border_pixels for scalar in line[0]]
                 end = [scalar * CONFIG.ppi + CONFIG.border_pixels for scalar in line[1]]
                 pygame.draw.line(canvas, COLOR, start, end, THICKNESS)
 
     def generate_floor(self):
         '''Generates the floor of the maze'''
 
-        if not self.wall_squares:
+        if not self.reduced_walls:
             sys.exit('Walls must be imported before a floor pattern is generated.')
 
         # Get the number of floor checker points

robot.py

@@ -27,7 +27,8 @@
     K_s,
     K_d,
     K_q,
-    K_e
+    K_e,
+    K_t
 )
 import config as CONFIG
 import utilities
@@ -163,6 +164,12 @@ def move_manual(self, keypress, walls):
         if keypress[K_a]:
             rotation += -rotation_speed
 
+        # Teleportation test
+        # if keypress[K_t]:
+        #    teleport_success = self.teleport(10, 10, 0, walls)
+        #    if not teleport_success:
+        #        print("Teleport failed due to collision.")
+
         # Move the robot
         self.move(move_vector, rotation, walls)
 
@@ -190,12 +197,40 @@ def move(self, velocity, rotation, walls):
         self.update_outline()
 
         # Reset the position if a collision is detected
-        collisions = self.check_collision_walls(walls)
+        collisions = self.check_collision_walls_fast(walls)
         if collisions:
             self.position -= pm.Vector2.rotate(velocity, self.rotation)
             self.rotation -= rotation
             self.update_outline()
 
+    def teleport(self, x, y, angle, walls):
+        '''Attempts to teleport the robot to a location,
+        returns True if successful
+        if collision, reverts to previous location and returns False'''
+
+        original_position = [self.position, self.rotation]
+
+        self.position = pm.Vector2(x, y)
+        self.rotation = angle
+        self.update_outline()
+        print(CONFIG.maze_dim_x, CONFIG.maze_dim_y)
+
+        # Returns False if the selected position is outside of the bounds of the map
+        if not (0 < self.position.x < CONFIG.maze_dim_x and 0 < self.position.y < CONFIG.maze_dim_y):
+            self.position = original_position[0]
+            self.rotation = original_position[1]
+            self.update_outline()
+            return False
+
+        # Returns True if the robot isn't inside a block and if there's no intersection with walls.
+        if not utilities.in_block(self.position) and not self.check_collision_walls_fast(walls):
+            return True
+        else:
+            self.position = original_position[0]
+            self.rotation = original_position[1]
+            self.update_outline()
+            return False
+
     def stop_drives(self):
         '''Stops all drives from moving, used as an emergency stop.'''
         for drive in self.drives.values():
@@ -215,6 +250,25 @@ def check_collision_walls(self, walls: list):
                     if collision_points:
                         return collision_points
 
+    def check_collision_walls_fast(self, walls: list)->bool:
+        '''
+        Checks for a collision between the robot's perimeter segments
+        and a set of wall line segments.
+        '''
+
+        # Loop through all the robot outline line segments, checking for collisions
+        for segment_bot in self.outline_global_segments:
+            for segment_wall in walls:
+                collides = utilities.check_collision_fast(
+                    segment_bot, segment_wall
+                )  # bool value
+                if collides:
+                    return True
+
+        return False
+
+
+
     def command(self, cmds: list, environment: dict):
         '''
         Parse text string of commands and act on them, sending them to the appropriate

simmer.py

@@ -95,9 +95,9 @@
 
         # Move the robot, either from keypress commands or from the movement buffers
         if True in keypress:
-            ROBOT.move_manual(keypress, [BLOCK.block_square, *MAZE.wall_squares])
+            ROBOT.move_manual(keypress, [*BLOCK.block_square, *MAZE.reduced_walls])
         else:
-            ROBOT.move_from_command([BLOCK.block_square, *MAZE.wall_squares])
+            ROBOT.move_from_command([*BLOCK.block_square, *MAZE.reduced_walls])
 
         # Recalculate global positions of the robot and its devices
         ROBOT.update_outline()