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 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():
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():
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 rrtPlannedPath(start_node, goal_node, robot_radius, plotter, write=False):
step_size = robot_radius # *2
rrt_nodes = {start_node.getXYCoords(): start_node}
step_node = start_node
itr = 0
while (not utils.sameRegion(step_node, goal_node,
GOAL_REACH_THRESH)) and (itr < MAX_ITER):
# print("Iteration number:", itr)
itr += 1
# get random node in the direction of the goal node 40% of the times.
rand_node = rrt.getRandomNode(x_lim=X_LIM,
y_lim=Y_LIM,
curr_node=start_node,
goal_node=goal_node,
goal_probability=0.6)
if plotter is not None:
utils.plotPoint(rand_node.getXYCoords(),
plotter,
radius=0.03,
color='red')
closest_node, _ = rrt.findClosestNode(rrt_nodes.values(), rand_node)
step_node = rrt.getStepNode(closest_node, rand_node, step_size)
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='blue')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='blue')
# plt.show()
# plt.pause(0.5)
if rrt.hitsObstacle(start_node=closest_node,
goal_node=step_node,
step_size=(robot_radius / 2)):
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='red')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='red')
# plt.show()
# plt.pause(0.5)
continue
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='green')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='green')
# plt.show()
# plt.pause(0.5)
rrt_nodes.update({step_node.getXYCoords(): step_node})
if plotter is not None:
plt.show()
plt.pause(0.05)
if write:
plt.savefig('./frames/' + str(itr) + '.png')
# Reached Goal
if utils.sameRegion(step_node, goal_node, GOAL_REACH_THRESH):
print("Reached Goal!")
print("Number of iterations:", itr)
goal_node.parent_coords = closest_node.getXYCoords()
goal_node.distance = utils.euclideanDistance(
point_1=closest_node.getXYCoords(),
point_2=goal_node.getXYCoords())
rrt_nodes.update({goal_node.getXYCoords(): goal_node})
# rrt_nodes.append(goal_node)
path = rrt.backtrack(rrt_nodes, goal_node)
print("path:", len(path), "rrt_nodes:", len(rrt_nodes))
return (path, rrt_nodes, itr)
return (None, None, None)
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 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
