Use matplotlib to simulate a 2D environment with static obstacles, the simulated environment can be used to test robotics path planning algorithms.
size_x, size_y: width, height of the environment
n_obs: number of obstacles (optional)
resolution: resolution of the configuration space (optional)
random_seed: to replicate result (optional)
self.size_x, self.size_y, self.resolution
self.obs: A list storing obstacles as Polygon object
self.cspace: A 2D np.array of x,y (row, col) configuration space, True for free-collision, False otherwise
self.x_grid, self.y_grid: Store x, y values for configuration space
generate_cspace(self, resolution=0.1): Generate self.cspace, self.x_grid, self.y_grid of the environment
check_collision(self, point, buffer=0.1): Check if a point (x, y) collides with the obstacles with buffer
check_line_collision(self, p1, p2, step=100, buffer=0.1): Check if line segment p1p2 collides with the obstacles with buffer
add_polyobs(self, vertices): Add a polygon obstacle into the environment with vertices arranged in a sequenced list
generate_obstacles(self, n_obs, max_vertices=6): Generate random number of obstacles in the environment
generate_start_goal(self): Generate random start and goal point (x, y)
xy_to_idx(self, xy): Convert point (x, y) into corresponding idx (i, j) in cspace
is_valid_move(self, idx): Check if a point (x, y) is valid for movement
ifReachGoal(self, xy): Check if a point (x, y) reaches goal (if Euclidean distance < 0.01)
check_line_coverage(p1, p2): Check the grid indices covered by a line, return a set of grid indices and True if no collision occur, False otherwise
evaluate_performance(self, sol_path, visited_idx, output_file='performance_metrics.txt'): Evaluate the performance of a solution path considering path cost, coverage area, goal reaching, completeness, output metrics in txt file
visualize(self, sol_path): Visualize the solution path in the environment. Obstacles are of red edges, start point is of yellow triangle, goal point is of green star, current point is of blue dot. Occupied grid cells are highlighted red, covered cells are highlighted green.
- Clone this repo
git clone https://github.com/bk1021/roomba-path-planning-simulation
- Create your path planning python script inside the directory
- In python file
from simulation import Environment
env = Environment((5, 5), random_seed=66)
def planner(env):
# your planning algo
...
return sol_path
env.visualize(planner(env))