def test_rrtstar():
general_obstacles_list = [
[
(1, 1),
(2, 1),
(2, 2),
(1, 2),
],
[(3, 4), (4, 4), (4, 5), (3, 5)],
]
poly = PolygonEnv(buffer_dist=0.1)
poly.update(general_obstacles_list)
sampler = UniformRectSampler(0, 6, 0, 6)
my_tree = RRTstar(
sampler=sampler,
expand_dis=0.1,
neighbourhood_radius=0.5,
goal_distance=0.5,
max_iter=2000,
)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(5, 5))
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.1)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sampler=sampler, r=5, n=100)
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.5)
my_tree = RRT(sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def prmstar_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
polygon = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_prmstar = PRMStar(sampler=sampler, c=30, n=100)
# Plan a path using the PRMStar.plan method for each obstacle:
for obstacles in general_obstacles_list:
polygon.update(obstacles) # updates the environment with the obstacle
path = my_prmstar.plan(start, goal, polygon)
visualize_path(path, polygon)
if len(path.path) != 1: # check if the path has only the start state
assert polygon.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
# poly.update(general_obstacles_list)
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate rrt planner object
my_tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def test_potential_field():
obstacle_list = [[(1, 1), (1.5, 1.5), (1.5, 1)]]
env = PolygonEnv(buffer_dist=0.1)
env.update(obstacle_list)
my_planner = PotentialField(KP=5, ETA=100, THRESH=15, STEP_SIZE=0.1, error=0.2)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(7, 7))
path, _ = my_planner.plan(start, goal, env)
assert env.get_traj_status(path) is True
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate prm constructer object
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sample_area=(-5, 15), sampler=sampler, r=5, n=100)
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True