def main():
start_time = time.time()
start_node = node_rrt.Node_rrt(current_coords=(-5001.787385427393, 9526.705177228898), parent_coords=None, distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(-2833, 4969), parent_coords=None, distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-6000, -1000)
ax.set_ylim(4000, 10000)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
plt.ioff()
plt.show()
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test.txt', 'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
def main():
start_node = node.Node(current_coords=(-4, -4),
parent_coords=None,
distance=0)
goal_node = node.Node(current_coords=(4, 4),
parent_coords=None,
distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-6, 6)
ax.set_ylim(-6, 6)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
robot_radius=DRONE_RADIUS,
plotter=ax,
write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
plt.ioff()
plt.show()
def initPlot(start_xy, goal_xy, title=""):
"""
Initializes the plot.
:param start_xy: The start xy node
:type start_xy: tuple
:param goal_xy: The goal xy node
:type goal_xy: tuple
:param title: frame title
:type title: string
:returns: Plotter
:rtype: matplotlib.pyplot
"""
fig, ax = plt.subplots()
fig.suptitle(title, fontsize=16)
ax.set(xlim=(-5, 5), ylim=(-5, 5))
ax.set_aspect('equal')
obstacles.generateMap(plotter=ax)
markNodeXY(start_xy, plotter=ax, color='#00FF00', marker='o')
markNodeXY(goal_xy, plotter=ax, color='#FF0000', marker='^')
return ax
def visualizeNewPath(BC_x, BC_y, point_list, animate=False, write_path=None, itr=-1):
fig, ax = plt.subplots()
fig.gca().set_aspect('equal', adjustable='box')
ax.set_xlim(-6, 6)
ax.set_ylim(-6, 6)
obs.generateMap(plotter=ax)
utils.plotPoints(point_list, ax, radius=0.04, color='black') # all points
utils.plotPoint(point_list[0], ax, radius=0.06, color='cyan') # start
utils.plotPoint(point_list[-1], ax, radius=0.06, color='magenta') # end
utils.plotPath(path=point_list, plotter=ax, path_color='black')
utils.plotPath(path=zip(BC_x, BC_y), plotter=ax, path_color='lime')
if write_path is not None:
if itr > -1:
plt.savefig(os.path.join(write_path, ('%04d.png' % itr)))
itr += 1
return itr
else:
plt.savefig(os.path.join(write_path, ('frame.png')))
plt.show()
if animate:
plt.pause(0.5)
def main():
json_data = []
with open('Map2.json', 'r') as f:
json_data = json.load(f)
reset_point_ = json_data.get('reset_point')
landing_point_ = json_data.get('landing_point')
reset_point = reset_point_.values()
landing_point = landing_point_.values()
start_time = time.time()
start_node = node_rrt.Node_rrt(current_coords=(reset_point[0], reset_point[1]), parent_coords=None, distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(landing_point[0],landing_point[1] ), parent_coords=None, distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-15000, 15000)
ax.set_ylim(-15000, 15000)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
plt.ioff()
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
rrt_prune_smooth_path_coords=path_pruning.prunedPath(path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS/2))
rrt_prune_smooth_path_coords= np.array(rrt_prune_smooth_path_coords[::-1])
plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan')
plt.show()
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_land_test.txt', 'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_land_test_smooth.txt', 'w') as fp:
fp.write('\n'.join('%s %s' % x for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def testMain():
fig, ax = plt.subplots()
obstacles.generateMap(ax)
ax.set_xlim(-6, 6)
ax.set_ylim(-6, 6)
fig.gca().set_aspect('equal', adjustable='box')
# path = np.load('./path.npy', allow_pickle=True)
# print(len(path))
# path_new = [[-4,-4],[-2, -4], [0,-4], [0, -3]]
path_new = np.array([[-4., -4.], [-3.79264131,
-3.6579439], [-3.46906898, -3.42278236],
[-3.48912813, -3.02328564], [-3.3948723, -2.63454943],
[-3.1296557, -2.33511637], [-2.89075655, -2.01429369],
[-2.7065285, -1.65924436], [-2.36537512, -1.45040382],
[-2.07738126,
-1.17280782], [-1.83184281, -0.85703754],
[-2.13718564, -0.59864683],
[-2.25697259, -0.21700421], [-2.17639411, 0.17479562],
[-1.89260448, 0.45668824], [-1.71137551, 0.81327772],
[-1.40609844, 1.07174612], [-1.00910694, 1.12071291],
[-0.63795093, 1.26985452], [-0.30719887, 1.49480579],
[-0.35420504, 1.89203421], [0.02984229, 2.0038718],
[0.42573924, 1.94672632], [0.47830175, 1.55019488],
[0.6912239, 1.21157391], [1.07448352, 1.097066],
[1.47007429, 1.03783814], [1.86993423, 1.02725365],
[2.22281226, 1.2156073], [2.43382091, 1.55542393],
[2.76428396, 1.3300483], [3.15622053, 1.40995904],
[3.22079987, 1.80471151], [3.27615393, 2.2008629],
[3.37145145, 2.58934505], [3.63297828, 2.89200612],
[3.7511519, 3.27415137], [3.50783803, 3.59163894],
[4., 4.]])
# path_new = [[-4,-3],[-3,-4], [-2,-4.5], [-0.5,-4], [0,-2],[2,0]]
pr_path = prunedPath(path=path_new)
pr_path = np.array(pr_path)
plt.plot(path_new[:, 0], path_new[:, 1], 'g')
plt.plot(pr_path[:, 0], pr_path[:, 1], 'r')
plt.show()
def main():
json_data = []
with open('Map2.json', 'r') as f:
json_data = json.load(f)
start_point = json_data.get('start_point')
print(start_point.values())
# find nearest point
uav_location = start_point.values()
with open(
"/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_cpp.txt",
"r") as ins:
path_cpp = []
for line in ins:
path_cpp.append([float(line) for line in line.split()
]) # here send a list path_cpp
samples = np.array(path_cpp)
samples.sort(kind='quicksort')
tree = KDTree(samples)
uav_location_ = np.array([uav_location]) #here pud a goal pos after rrt
closest_dist, closest_id = tree.query(
uav_location_, k=1) # closest point with respect to the uav location
point_of_entery = samples[closest_id]
new_path_cpp = path_cpp[int(closest_id):]
new_path_cpp.extend(path_cpp[:int(closest_id)])
del path_cpp[0:int(closest_id)]
path_cpp_of_tuples = [tuple(l) for l in path_cpp]
print(point_of_entery)
#print(int(closest_id))
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_cpp_.txt',
'w') as fp:
fp.write('\n'.join('%s %s' % x for x in path_cpp_of_tuples))
# rrt path planning
start_time = time.time()
start_node = node_rrt.Node_rrt(current_coords=(uav_location[0],
uav_location[1]),
parent_coords=None,
distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(point_of_entery[0, 0, 0],
point_of_entery[0, 0, 1]),
parent_coords=None,
distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-15000, 15000)
ax.set_ylim(-15000, 15000)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
STEP_SIZE,
plotter=ax,
write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
plt.ioff()
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
rrt_prune_smooth_path_coords = path_pruning.prunedPath(
path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2))
rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1])
plt.plot(rrt_prune_smooth_path_coords[:, 0],
rrt_prune_smooth_path_coords[:, 1], 'cyan')
plt.show()
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
#print(rrt_path_coords)
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test.txt',
'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test_smooth.txt',
'w') as fp:
fp.write('\n'.join(
'%s %s' % x
for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def main():
start_node = node.Node(current_coords=(-4, -4),
parent_coords=None,
distance=0)
goal_node = node.Node(current_coords=(4, 4),
parent_coords=None,
distance=0)
start1 = start_node.getXYCoords()
quat = [0, 0, 0, 1]
state_msg = ModelState()
state_msg.model_name = 'iris'
state_msg.pose.position.x = start1[0]
state_msg.pose.position.y = start1[1]
state_msg.pose.position.z = 0
state_msg.pose.orientation.x = quat[0]
state_msg.pose.orientation.y = quat[1]
state_msg.pose.orientation.z = quat[2]
state_msg.pose.orientation.w = quat[3]
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state(state_msg)
fig, ax = plt.subplots()
ax.set_xlim(-6, 6)
ax.set_ylim(-6, 6)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
robot_radius=DRONE_RADIUS,
plotter=None,
write=False)
itr = 0
# plt.savefig(('./frames/%04d.png' % (itr))); itr += 1
# print(rrt_path)
if rrt_path is not None:
itr = utils.plotPath(rrt_path, plotter=ax, itr=itr, path_color='black')
itr += 1
# plt.ioff()
# plt.show()
# Pruning
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
print(path_co.shape)
rrt_prune_smooth_path_coords = path_pruning.prunedPath(
path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2))
rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1])
# plt.plot(path_co[:,0],path_co[:,1],'green')
# plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan')
# if path_co is not None:
# utils.plotPath(path_co, plotter=ax, itr=itr, path_color='black')
if rrt_prune_smooth_path_coords is not None:
itr = utils.plotPath(rrt_prune_smooth_path_coords,
plotter=ax,
itr=itr,
path_color='cyan')
itr += 1
plt.ioff()
def main():
json_data = []
with open('Map2.json', 'r') as f:
json_data = json.load(f)
start_point_ = json_data.get('start_point')
#centroid_ = json_data.get('centroid')
'''json_data_2 = []
with open('Scenario2.json', 'r') as d:
json_data_2 = json.load(d)
target_point = json_data_2.get('centroid')'''
x = [
-957.3915945077315, -910.8959478922188, -948.4347058273852,
-875.1556231221184, -845.2041011163965, -858.6145041380078,
-919.3042095946148, -942.3035844964907, -933.9325257679448,
-889.955683006905
]
y = [
-855.0138749238104, -895.5183857586235, -921.3926183544099,
-947.264425364323, -858.3795844987035, -861.4421726837754,
-895.0775583777577, -844.3298955513164, -922.8892891705036,
-887.7070486117154
]
centroid = [sum(x) / len(x), sum(y) / len(y)]
print(centroid)
start_point = start_point_.values()
#target_point = centroid_.values()
print(type(start_point))
print(centroid)
start_time = time.time()
start_node = node_rrt.Node_rrt(current_coords=(start_point[0],
start_point[1]),
parent_coords=None,
distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(centroid[0], centroid[1]),
parent_coords=None,
distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-15000, 15000)
ax.set_ylim(-15000, 15000)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
STEP_SIZE,
plotter=ax,
write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
plt.ioff()
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
rrt_prune_smooth_path_coords = path_pruning.prunedPath(
path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2))
rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1])
plt.plot(rrt_prune_smooth_path_coords[:, 0],
rrt_prune_smooth_path_coords[:, 1], 'cyan')
plt.show()
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_test_scenario2.txt',
'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_test__scenario2_smooth.txt',
'w') as dp:
dp.write('\n'.join(
'%s %s' % x
for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def a_star(start_rc,
goal_rc,
orientation,
rpm1=10,
rpm2=20,
clearance=0.2,
viz_please=False):
"""
A-star algorithm given the start and the goal nodes.
:param start_rc: The start position
:type start_rc: tuple
:param goal_rc: The goal position
:type goal_rc: tuple
:param orientation: The orientation
:type orientation: number
:param rpm1: The rpm 1
:type rpm1: number
:param rpm2: The rpm 2
:type rpm2: number
:param clearance: The clearance
:type clearance: number
:param viz_please: Whether to visualize or not
:type viz_please: boolean
:returns: Returns path nodes and the list of visited nodes.
:rtype: tuple
"""
"""
Inputs
"""
start_node = node.Node(current_coords=start_rc,
parent_coords=None,
orientation=0,
parent_orientation=None,
action=None,
movement_cost=0,
goal_cost=utils.euclideanDistance(
start_rc, goal_rc))
print("-----------------------")
print("Start Node:")
start_node.printNode()
print("-----------------------")
goal_node = node.Node(current_coords=goal_rc,
parent_coords=None,
orientation=None,
parent_orientation=None,
action=None,
movement_cost=None,
goal_cost=0)
"""
Initializations
"""
action_set = [(0, rpm1), (rpm1, 0), (0, rpm2), (rpm2, 0), (rpm1, rpm2),
(rpm2, rpm1), (rpm1, rpm1), (rpm2, rpm2)]
min_heap = [((start_node.movement_cost + start_node.goal_cost), start_node)
]
heapq.heapify(min_heap)
visited = {}
visited.update({
(utils.getKey(start_rc[0], start_rc[1], start_node.orientation)):
start_node
})
visited_viz_nodes = [start_node]
"""
Initialize the plot figures if the visualization flag is true.
Also mark the start and the goal nodes.
"""
if viz_please:
fig, ax = plt.subplots()
ax.set(xlim=(an.MIN_COORDS[0], an.MAX_COORDS[0]),
ylim=(an.MIN_COORDS[1], an.MAX_COORDS[1]))
# ax.set(xlim=(-5, 5), ylim=(-5, 5))
ax.set_aspect('equal')
obstacles.generateMap(plotter=ax)
viz.markNode(start_node, plotter=ax, color='#00FF00', marker='o')
viz.markNode(goal_node, plotter=ax, color='#FF0000', marker='^')
plt.ion()
"""
Run the loop for A-star algorithm till the min_heap queue contains no nodes.
"""
while (len(min_heap) > 0):
_, curr_node = heapq.heappop(min_heap)
# Consider all the action moves for all the selected nodes.
for action in action_set:
new_node = an.actionMove(current_node=curr_node,
next_action=action,
goal_position=goal_rc,
clearance=clearance)
# Check if all the nodes are valid or not.
if (new_node is not None):
"""
Check if the current node is a goal node.
"""
if new_node.goal_cost < GOAL_REACH_THRESH:
print("Reached Goal!")
print("Final Node:")
new_node.printNode()
visited_viz_nodes.append(new_node)
path = an.backtrack(new_node, visited)
print("------------------------")
if viz_please:
viz.plot_curve(new_node, plotter=ax, color="red")
viz.plotPath(path,
rev=True,
pause_time=0.5,
plotter=ax,
color="lime",
linewidth=4)
plt.ioff()
plt.show()
return (path, visited_viz_nodes)
"""
Mark node as visited,
Append to min_heap queue,
Update if already visited.
"""
node_key = (utils.getKey(new_node.current_coords[0],
new_node.current_coords[1],
new_node.orientation))
if node_key in visited:
if new_node < visited[node_key]:
visited[node_key] = new_node
min_heap.append(
((new_node.movement_cost + new_node.goal_cost),
new_node))
else:
if viz_please:
viz.plot_curve(new_node, plotter=ax, color="red")
plt.show()
plt.pause(0.001)
visited.update({node_key: new_node})
min_heap.append(
((new_node.movement_cost + new_node.goal_cost),
new_node))
visited_viz_nodes.append(new_node)
# Heapify the min heap to update the minimum node in the list.
heapq.heapify(min_heap)
def main():
json_data = []
if os.path.exists('Map2.json'):
with open('Map2.json', 'r') as f:
json_data = json.load(f)
reset_point = json_data.get('reset_point')
drop_location = reset_point.values()
else:
print('####!!!!There is no file with mission!!!!####')
json_data_2 = []
x_arr = []
y_arr = []
if os.path.exists('Targets.json'):
with open('Targets.json', 'r') as d:
json_data_2 = json.load(d)
target_point_arr = json_data_2.get('points')
for i in range(len(target_point_arr)):
target_point_arr_ = target_point_arr[i]
x = target_point_arr_[0]
y = target_point_arr_[1]
x_arr.append(x)
y_arr.append(y)
centroid = [sum(x_arr) / len(x_arr), sum(y_arr) / len(y_arr)]
else:
print('####!!!!There is no file with targets!!!!####')
start_time = time.time()
start_node = node_rrt.Node_rrt(current_coords=(centroid[0], centroid[1]),
parent_coords=None,
distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(drop_location[0],
drop_location[1]),
parent_coords=None,
distance=0)
fig, ax = plt.subplots()
ax.set_xlim(-15000, 15000)
ax.set_ylim(-15000, 15000)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
# Set plotter=None here to disable animation
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
STEP_SIZE,
plotter=ax,
write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
plt.ioff()
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
rrt_prune_smooth_path_coords = path_pruning.prunedPath(
path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS))
rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1])
plt.plot(rrt_prune_smooth_path_coords[:, 0],
rrt_prune_smooth_path_coords[:, 1], 'cyan')
plt.show()
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/px4_controller/rrt_pruning_smoothing/Code/path_rrt_reset_test_scenario2.txt',
'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/px4_controller/rrt_pruning_smoothing/Code/path_rrt_reset_test_smooth_scenario2.txt',
'w') as fp:
fp.write('\n'.join(
'%s %s' % x
for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def bezierCurveTesting1(point_list, animate=False, write_path=None):
fig, ax = plt.subplots()
fig.gca().set_aspect('equal', adjustable='box')
ax.set_xlim(-6, 6)
ax.set_ylim(-6, 6)
obs.generateMap(plotter=ax)
utils.plotPoints(point_list, ax, radius=0.04, color='black') # all points
utils.plotPoint(point_list[0], ax, radius=0.06, color='cyan') # start
utils.plotPoint(point_list[-1], ax, radius=0.06, color='magenta') # end
if write_path is not None:
write_itr = 0
plt.savefig(write_path + '/%s.png' % (str(write_itr)))
write_itr += 1
if animate:
plt.ion()
# List of all path points
BC_x = []
BC_y = []
# Initial points
try:
P0 = point_list[0]
P1 = point_list[1]
P2 = point_list[2]
P3 = point_list[3]
except Exception:
print("Insufficient points. Appropriate case handled later.")
# Starting Curve
if len(point_list) > 4:
bc_x, bc_y = bc.bezierCubicCurveEquation(P0,
P1,
P2, ((P2 + P3) / 2),
step_size=0.01)
elif len(point_list) == 4:
bc_x, bc_y = bc.bezierCubicCurveEquation(P0,
P1,
P2,
P3,
step_size=0.01)
elif len(point_list) == 3:
bc_x, bc_y = bc.bezierQuadraticCurveEquation(P0,
P1,
P2,
step_size=0.01)
elif len(point_list) == 2:
bc_x, bc_y = bc.bezierLinearCurveEquation(P0, P1, step_size=0.01)
BC_x.extend(bc_x)
BC_y.extend(bc_y)
# Animation
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red')
plt.plot(bc_x, bc_y, color='green')
plt.show()
plt.pause(1)
if write_path is not None:
plt.savefig(write_path + '/%s.png' % (str(write_itr)))
write_itr += 1
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow')
##############
# Handling the rest of the points, leaving out the last 1 or 2 points for the end
point_idx = 4
if point_idx >= len(point_list):
plt.ioff()
# plt.show()
return (BC_x, BC_y)
while point_idx + 2 < len(point_list):
P0 = P2
P1 = P3
P2 = point_list[point_idx]
point_idx += 1
P3 = point_list[point_idx]
point_idx += 1
bc_x, bc_y = bc.bezierCubicCurveEquation(((P0 + P1) / 2), P1, P2,
((P2 + P3) / 2), 0.01)
BC_x.extend(bc_x)
BC_y.extend(bc_y)
# Animation
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red')
plt.plot(bc_x, bc_y, color='green')
plt.show()
plt.pause(1)
if write_path is not None:
plt.savefig(write_path + '/%s.png' % (str(write_itr)))
write_itr += 1
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow')
##############
# Ending Curve
P0 = P2
P1 = P3
P2 = point_list[point_idx]
point_idx += 1
if point_idx < len(point_list):
P3 = point_list[point_idx]
bc_x, bc_y = bc.bezierCubicCurveEquation(((P0 + P1) / 2),
P1,
P2,
P3,
step_size=0.01)
else:
bc_x, bc_y = bc.bezierQuadraticCurveEquation(((P0 + P1) / 2),
P1,
P2,
step_size=0.01)
BC_x.extend(bc_x)
BC_y.extend(bc_y)
# Animation
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red')
plt.plot(bc_x, bc_y, color='green')
plt.show()
plt.pause(1)
if write_path is not None:
plt.savefig(write_path + '/%s.png' % (str(write_itr)))
write_itr += 1
if animate:
utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow')
##############
# Animation
if animate:
plt.ioff()
else:
plt.plot(BC_x, BC_y, color='green')
# plt.show()
if write_path is not None:
plt.savefig(write_path + '/%s.png' % (str(write_itr)))
##############
return (BC_x, BC_y)
def main():
prm.main()
""" rospy.loginfo("This start x pose: %s", start_pose_x)
rospy.loginfo("This start y pose: %s", start_pose_y)
rospy.loginfo("This goal x pose: %s", goal_pose_x)
rospy.loginfo("This goal y pose: %s", goal_pose_y)"""
#possition_listener()
if (prm.start_pose_x == None) or (prm.start_pose_y == None):
rospy.loginfo("Problem with start coordinates!")
rospy.loginfo("Start x: '%s'" + " and " + "y: '%s' coordinates: ",
str(prm.start_pose_x), str(prm.start_pose_y))
sys.exit(0)
elif (prm.goal_pose_x == None) or (prm.goal_pose_y == None):
rospy.loginfo("Problem with goal coordinates!")
rospy.loginfo("Goal x: '%s'" + " and " + "y: '%s' coordinates: ",
str(prm.goal_pose_x), str(prm.goal_pose_y))
sys.exit(0)
start_node = node_rrt.Node_rrt(current_coords=(prm.start_pose_x,
prm.start_pose_y),
parent_coords=None,
distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(prm.goal_pose_x,
prm.goal_pose_y),
parent_coords=None,
distance=0)
start1 = start_node.getXYCoords()
quat = [0, 0, 0, 1]
state_msg = ModelState()
state_msg.model_name = 'iris'
state_msg.pose.position.x = start1[0]
state_msg.pose.position.y = start1[1]
state_msg.pose.position.z = 0
state_msg.pose.orientation.x = quat[0]
state_msg.pose.orientation.y = quat[1]
state_msg.pose.orientation.z = quat[2]
state_msg.pose.orientation.w = quat[3]
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state(state_msg)
fig, ax = plt.subplots()
ax.set_xlim(prm._lim_x, prm.lim_x)
ax.set_ylim(prm._lim_y, prm.lim_y)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
robot_radius=DRONE_RADIUS,
plotter=None,
write=False)
itr = 0
# plt.savefig(('./frames/%04d.png' % (itr))); itr += 1
# print(rrt_path)
if rrt_path is not None:
itr = utils.plotPath(rrt_path, plotter=ax, itr=itr, path_color='black')
itr += 1
# plt.ioff()
# plt.show()
# Pruning
path_co = np.array(utils.convertNodeList2CoordList(rrt_path))
print(path_co.shape)
rrt_prune_smooth_path_coords = path_pruning.prunedPath(
path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2))
rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1])
# plt.plot(path_co[:,0],path_co[:,1],'green')
# plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan')
# if path_co is not None:
# utils.plotPath(path_co, plotter=ax, itr=itr, path_color='black')
if rrt_prune_smooth_path_coords is not None:
itr = utils.plotPath(rrt_prune_smooth_path_coords,
plotter=ax,
itr=itr,
path_color='cyan')
itr += 1
#rrt_smoothed_path_coors = utils.convertNodeList2CoordList(node_list=rrt_prune_smooth_path_coords)
#smoothed_path_length = utils.path_length_meters(rrt_smoothed_path_length)
smoothed_path_pength = utils.path_length_meters(
rrt_prune_smooth_path_coords)
try:
capacity = float(str(prm.capacity)[5:])
max_length = float(str(prm.max_length)[5:])
max_uav_path = capacity * max_length
if max_uav_path < path_length:
print("\nWarning!")
print("Path cannot be overcomed!")
else:
print("Path can be overcomed!")
except:
print("Capacity and maximum path lenght was not entered!")
with open('path.txt', 'w') as f:
f.write(json.dumps(rrt_path_coords))
plt.ioff()
plt.show()
#plt.savefig(('./frames/%04d.png' % (itr))); itr += 1
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
def main():
start_time = time.time()
prm.main()
print(X_LIM)
#possition_listener()
if (prm.start_pose_x == None) or (prm.start_pose_y == None):
rospy.loginfo("Problem with start coordinates!")
rospy.loginfo("Start x: '%s' and y: '%s' coordinates",
str(prm.start_pose_x), str(prm.start_pose_y))
sys.exit(0)
elif (prm.goal_pose_x == None) or (prm.goal_pose_y == None):
rospy.loginfo("Problem with goal coordinates!")
rospy.loginfo("Goal x: '%s' and y: '%s' coordinates",
str(prm.goal_pose_x), str(prm.goal_pose_y))
sys.exit(0)
start_node = node_rrt.Node_rrt(current_coords=(prm.start_pose_x,
prm.start_pose_y),
parent_coords=None,
distance=0)
goal_node = node_rrt.Node_rrt(current_coords=(prm.goal_pose_x,
prm.goal_pose_y),
parent_coords=None,
distance=0)
#start_node = node_rrt.Node_rrt(current_coords=(0, 0), parent_coords=None, distance=0)
#goal_node = node_rrt.Node_rrt(current_coords=(5, 5), parent_coords=None, distance=0)
fig, ax = plt.subplots()
ax.set_xlim(prm._lim_x, prm.lim_x)
ax.set_ylim(prm._lim_y, prm.lim_y)
fig.gca().set_aspect('equal', adjustable='box')
utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06,
color='cyan') # start
utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06,
color='magenta') # end
#obs.testMain()
obs.generateMap(ax)
plt.ion()
rrt_path, _, itr = rrtPlannedPath(start_node,
goal_node,
robot_radius=prm.c_drone_radius,
plotter=ax,
write=False)
if rrt_path is not None:
utils.plotPath(rrt_path, plotter=ax)
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
path_length = utils.path_length_meters(rrt_path_coords)
try:
capacity = float(str(prm.capacity)[5:])
max_length = float(str(prm.max_length)[5:])
max_uav_path = capacity * max_length
if max_uav_path < path_length:
print("\nWarning!")
print("Path cannot be overcomed!")
else:
print("Path can be overcomed!")
except:
print("Capacity and maximum path lenght was not entered!")
finish_time = time.time()
result = finish_time - start_time
print("Program time: " + str(result) + " seconds.")
#global rrt_path_coords_c
#rrt_path_coords_c = []
#rrt_path_coords_c = utils.convertNodeList2CoordList(node_list=rrt_path)
rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
with open(
'/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_.txt',
'w') as fp:
fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
#f = open('path.txt', 'w')
#for element in rrt_path_coords:
#line = ' '.join(str(x) for x in element)
#f.write(line + '\n')
#f.close()
#with open('path.txt', 'w') as f:
# for item in rrt_path_coords:
# f.write(json.dumps(rrt_path_coords))
plt.ioff()
plt.show()
sys.exit