def main():
weights = [0, 10, 10]
start_cartesian = create_cartesian((33.446056, -118.489111),
catalina.ORIGIN_BOUND)
print(start_cartesian)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES,
catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
habitat_list = environ[3]
astar_solver = astar(start, obstacle_list + boat_list, boundary_list)
final_path_mps = astar_solver.astar(habitat_list,
obstacle_list + boat_list,
boundary_list, start, 400, weights)
print("\n", "final trajectory: ", final_path_mps[0])
print("\n", "cost of each node on the final trajectory: ",
final_path_mps[1])
def generate_random_start_pos(self):
"""
Generate a random starting position within bounds and collision-free
Parameters:
NONE
Returns:
A position tuple (x, y)
"""
# Set up the environment
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0] + environ[2]
boundary_list = environ[1]
# Initialize doable to Flase
doable = False
while doable == False:
random_start_position = (random.randint(-500, 100), random.randint(-200, 200))
# check if no collision
if self.collision_free(random_start_position, obstacle_list):
# check if within bounds
if self.within_bounds(boundary_list, random_start_position):
doable = True
return random_start_position
if doable == False:
return (-100,0)
def sharks_vs_costTime(times, start, XLIST, weights, pathLenLimit, total_traj_time):
"""
Keep the information of shark trajectories and habitats constant to see the association between the number of obstacles and cost
Parameter:
times: an integer represents the number of iterations
start: a position tuple of two elements in the system of longtitudes and latitudes
XLIST: a list of integers each of which signifies the complexity level(i.e. item account)
shark_dict: a list of Motion_plan_state objects; represents multiple shark trajectories
weights: a list of four integers; we use [0, 10, 10, 100]
total_traj_time: in seconds; set it to 200s
"""
COST = []
TIME = []
start_cartesian = catalina.create_cartesian(start, catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
habitat_list = environ[3]
boundary_list = environ[1]
obstacle_list = environ[0]+environ[2]
for complexity in XLIST: # for each kind of environment
cost_list = []
time_list = []
for sim in range(times): # run a number of simulations
shark_dict = build_sharkDict(complexity)
astar_solver = astar(start, obstacle_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1)
start_time = timeit.timeit()
final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict)
end_time = timeit.timeit()
peri_boundary = cal_boundary_peri(astar_solver)
traj_node = final_path_mps["node"]
traj_mps = final_path_mps["path"]
print ("\n", "Final trajectory : ", traj_mps)
cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, habitat_list, astar_solver.sharkGrid, weights)
cost_list.append(cost)
print ("\n", "cost list: ", cost_list)
time_list.append(abs(end_time-start_time))
print ("\n", "time list: ", time_list)
if len(cost_list) >= 1:
COST.append(mean(cost_list))
print ("\n","cost values: ", COST)
if len(time_list) >= 1:
TIME.append(mean(time_list))
print("\n", "time values: ", TIME)
return {"cost" : COST, "time" : TIME}
def main():
weights = [0, 10, 10, 100]
start_cartesian = create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
habitat_list = environ[3]
# testing data for shark trajectories
shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]}
astar_solver = astar(start, obstacle_list+boat_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1)
final_path_mps = astar_solver.astar(300, weights, shark_dict)
print ("\n", "Final Trajectory: ", final_path_mps["path"])
print ("\n", "Trajectory Length: ", final_path_mps["path length"])
print ("\n", "Trajectory Cost: ", final_path_mps["cost"])
print ("\n", "Trajectory Cost List: ", final_path_mps["cost list"])
boundary_poly = []
for pos in boundary_list:
boundary_poly.append((pos.x, pos.y))
boundary = Polygon(boundary_poly) # a Polygon object that represents the boundary of our workspace
sharkOccupancyGrid = SharkOccupancyGrid(shark_dict, 10, boundary, 50, 50)
grid_dict = sharkOccupancyGrid.convert()
plot(sharkOccupancyGrid, grid_dict[0], final_path_mps["path"])
def main():
weights = [0, 10, 50]
start_cartesian = create_cartesian((33.445089, -118.486933), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]+environ[2]
habitat_list = environ[3]
single_AUV = singleAUV(start, obstacle_list, boundary_list, habitat_list, [])
final_traj = single_AUV.astar(habitat_list, obstacle_list, boundary_list, start, 200, weights)
print ("\n", "final trajectory: ", final_traj["path"])
print ("\n", "Trajectory length: ", len(final_traj["path"]))
print ("\n", "cost of each node on the final trajectory: ", final_traj["cost"])
def display_single_astar_trajectory(self):
"""
Visualize the planned trajectory
Parameter:
NONE
Returns:
None
"""
start = (self.x, self.y)
# Create the environment
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]+environ[2]
habitat_list = environ[3]
# Create a single_AUV object
single_AUV = singleAUV(start, obstacle_list, boundary_list, habitat_list, [])
def multi_AUV_planner_fixed_start(self, pathLenLimit, weights, start_position_list):
"""
Find the optimal path for each AUV with fixed start positions
Parameter:
pathLenLimit -- the length limit of the planned trajectory in meters
weights -- a list of three coefficients [w1, w2, w3]
start_position_list -- a list of starting positions (x, y)
Returns:
dict -- a dictionary with "traj" matches a list of trajectory lists,
"costs" matches a list of costs corresponding to each trajectory list
"""
# Create environment
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
habitat_list = environ[3]
# Create permutations of the starting position list
start_pos_permu_list = permutations(start_position_list)
for AUV_travel_order_list in start_pos_permu_list:
self.multiAUV_habitat_open_list = habitat_list[:] # Reset the habitat coverage
self.multiAUV_habitat_closed_list = [] # All habitats are open
for start in AUV_travel_order_list:
print ("\n", "STARTING AT: ", start)
# Create a new object, single_AUV
single_AUV = singleAUV(start, self.obstacle_list, self.boundary_list, self.multiAUV_habitat_open_list, self.multiAUV_habitat_closed_list)
# Plan path for the single_AUV object
single_planner = single_AUV.astar(start, pathLenLimit, weights)
# Append the new path
self.trajectories.append(single_planner["path"])
# Append the new cost
self.costs.append(single_planner["cost"])
print("\n", "Traj: ", single_planner["path"])
# Update the overall habitat coverage
self.multiAUV_habitat_open_list = single_AUV.habitat_open_list[:]
self.multiAUV_habitat_closed_list = single_AUV.habitat_closed_list[:]
return {"trajs" : self.trajectories, "costs" : self.costs}
def display_multi_astar_trajectory(self, numAUV):
"""
Visualize multiple trajectories
Parameter:
numAUV -- the number of AUVs
Returns:
NONE
"""
# Generate a random starting position
random_start_pos = self.generate_random_start_pos()
print ("start at ", random_start_pos)
# Create the environment
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1] + environ[2]
habitat_list = environ[3]
# Create a multi_AUV object
AUVS = multiAUV(numAUV, random_start_pos, habitat_list, boundary_list, obstacle_list)
def iterate(times):
weights = [0, 10, 10, 100]
pathLenLimit = 200 # in meters
total_traj_time = 200 # in seconds
start_cartesian = catalina.create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND)
start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2))
print ("start: ", start)
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS)
obstacle_list = environ[0]
boundary_list = environ[1]
boat_list = environ[2]
x_list = [1, 3, 5, 7, 9]
y_list = [] # holds averaged costs
z_list = [] # holds averaged planning time
'''shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)],
10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]} '''
for complexity in x_list: # for each kind of environment
cost_list = []
time_list = []
for sim in range(times): # run a number of simulations
environ_dict = build_environ(complexity, start) # returns {"obstacles" : obstacles, "habitats" : habitats}
final_obstacle_list = obstacle_list+boat_list+environ_dict["obstacles"]
final_habitat_list = environ_dict["habitats"]
shark_dict = environ_dict["sharks"]
astar_solver = astar(start, final_obstacle_list, boundary_list, final_habitat_list, {}, shark_dict, AUV_velocity=1)
start_time = timeit.timeit()
final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict)
end_time = timeit.timeit()
peri_boundary = cal_boundary_peri(astar_solver)
traj_node = final_path_mps["node"]
traj_mps = final_path_mps["path"]
print ("\n", "Final trajectory : ", traj_mps)
cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, final_habitat_list, astar_solver.sharkGrid, weights)
cost_list.append(cost)
print ("\n", "cost list: ", cost_list)
time_list.append(abs(end_time-start_time))
print ("\n", "time list: ", time_list)
if len(cost_list) >= 1:
y_list.append(mean(cost_list))
print ("\n","y values: ", y_list)
if len(time_list) >= 1:
z_list.append(mean(time_list))
print("\n", "z values: ", z_list)
return {"Y" : y_list, "Z" : z_list}
end = time.time()
time_list.append((end - start))
res.append(statistics.mean(time_list))
return res
#initialize start, goal, obstacle, boundary, habitats for path planning
# start = catalina.create_cartesian(catalina.START, catalina.ORIGIN_BOUND)
# start = Motion_plan_state(start[0], start[1])
# goal = catalina.create_cartesian(catalina.GOAL, catalina.ORIGIN_BOUND)
# goal = Motion_plan_state(goal[0], goal[1])
# convert to environment in casrtesian coordinates
environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES,
catalina.BOATS, catalina.HABITATS)
obstacles = environ[0] + environ[2]
boundary = environ[1]
habitats = environ[3]
boundary_poly = [(mps.x, mps.y) for mps in boundary]
boundary_poly = Polygon(boundary_poly)
cell_list = splitCell(boundary_poly, 10)
# testing data for shark trajectories
shark_dict1 = {
1: [
Motion_plan_state(-120 + (0.2 * i), -60 + (0.2 * i), traj_time_stamp=i)
for i in range(1, 501)
],
