def testMain():
x_start, y_start = (-4, -4)
x_goal, y_goal = (0, -4)
start_coords = (x_start, y_start)
goal_coords = (x_goal, y_goal)
radius = 0.1
clearance = 0.1
path, goal_node = rrt(start_coords=start_coords,
goal_coords=goal_coords,
radius=radius,
clearance=clearance)
# print(path)
# np.save("../Paths/path.npy", path)
# np.save("./path.npy", path)
# sys.exit(0)
univ.function(exploration_node_vector=path,
goal_node=goal_node,
path_node_vector=None,
step_size=None)
path_pruning.prunedPath(path=path, goal_node=goal_node)
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 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 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)