def get_state(self, name):
""" Returns the actual state of a module
@:parameter name string (A module name)
@:returns <string> (A module state)
@:raise ModuleNotFoundError
"""
check_node(self._graph, name)
return self._nodes[name]["state"]
def get_dependencies(self, name):
""" Returns the dependency list of a module
@:parameter name string (A module name)
@:returns list<string> (A module names list)
@:raise ModuleNotFoundError
"""
check_node(self._graph, name)
subgraph = self._create_root_subgraph(self._graph, name)
return [node for node in subgraph.nodes()]
def get_optimized_modules_dependencies(self, path=None):
""" Returns the diff between actual dependencies and optimized ones
:param path: restrict to a specific submodules
:return:
"""
diff = {}
module_items = self._nodes.items()
# Doesn't keep uninstalled, uninstallable or to remove modules
graph = self._sub_graph_from_states(self._graph, [
States.TO_UPGRADE,
States.TO_INSTALL,
States.INSTALLED,
States.INSTALLABLE,
])
for module, values in module_items:
if not check_node(graph, module, raise_exception=False):
continue
# Check path only if provided
if path:
has_submodule = "submodule" in values and values["submodule"]
relpath = os.path.relpath(path, ".")
common_path = False
if has_submodule:
relsubpath = os.path.relpath(values["submodule"], ".")
common_path = relpath in relsubpath
if not common_path:
continue
children = [m for m, v in module_items if module in v["children"]]
predecessors = graph.predecessors(module)
if set(predecessors) != set(children):
diff[module] = sorted(predecessors)
return dict(sorted(diff.items()))
def valid_joint(p1, p2, clearance):
delta = np.linspace(0,
math.sqrt((p2[1] - p1[1])**2 + (p2[0] - p1[0])**2),
num=20)
theta = math.atan2(p2[1] - p1[1], p2[0] - p1[0])
points = []
for i in delta:
inter_point = (p1[0] + i * math.cos(theta),
p1[1] + i * math.sin(theta))
points.append(inter_point)
if not utils.check_node(inter_point, clearance):
return False
#print('Points', points)
return True
def child_generator(self, node, cost):
# List to store all valid children
valid_children = []
# Generating the children
n1 = self.move1(node, cost)
n2 = self.move2(node, cost)
n3 = self.move3(node, cost)
n4 = self.move4(node, cost)
n5 = self.move5(node, cost)
n6 = self.move6(node, cost)
n7 = self.move7(node, cost)
n8 = self.move8(node, cost)
# List to store all children
childs = [n1,n2,n3,n4,n5,n6,n7,n8]
# Dictionary with cost as key and child as value
childs_cost = {n1[3]:n1,n2[3]:n2,n3[3]:n3,n4[3]:n4, n5[3]:n5, n6[3]:n6,n7[3]:n7, n8[3]:n8}
# Check for valid children and append them to the list
for cost in childs_cost.keys():
if utils.check_node(childs_cost[cost], self.clearance) == True and visited_nodes[int(round(childs_cost[cost][0],1)/0.2)][int(round(childs_cost[cost][1],1)/0.2)][int(round(childs_cost[cost][2],1)/10)] == 0:
valid_children.append((cost, childs_cost[cost], childs_cost[cost][4], self.index(childs_cost[cost]),node))
valid_childs_dict[self.index(childs_cost[cost])] = [childs_cost[cost], node, self.index(node)]
visited_nodes[int(round(childs_cost[cost][0],1)/0.2)][int(round(childs_cost[cost][1],1)/0.2)][int(round(childs_cost[cost][2],1)/10)] = 1
return valid_children
def main():
rospy.init_node('turtlebot3_vel_publisher', anonymous=True)
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
vel_msg = Twist()
def callback(msg):
print(msg.pose.pose)
# Taking inputs from the user
time.sleep(2)
print('')
print(
"************************* Let's move the turtlebot! *******************************"
)
print('')
clearance = eval(
input(
'Please enter the clearance value of the robot from the obstacle:')
)
print('The clearance value you entered is:', clearance)
print('')
start_point = eval(
input(
'Please enter the start coordinates for the robot in this format - [X_coord, Y_coord, Theta]:'
))
while not utils.check_node(start_point, clearance):
start_point = eval(
input(
'Please enter the start coordinates in this format - [X_coord, Y_coord, Theta]:'
))
start_circle = plt.scatter(start_point[0], start_point[1], c='b')
print('The start point you entered is:', start_point)
print('')
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord]:'
))
while not utils.check_node(goal_point, clearance):
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord]:'
))
goal_circle = plt.scatter(goal_point[0], goal_point[1], c='y')
print('The goal point you entered is:', goal_point)
print('')
goal_circle = plt.Circle((goal_point[0], goal_point[1]),
radius=0.25,
fill=False)
plt.gca().add_patch(goal_circle)
rpm = eval(
input(
'Please enter the RPM for both the wheels in this format - [RPM1,RPM2]:'
))
print("The wheel RPM's you entered for both the wheels are:", rpm)
print('')
robot_radius = 0.089
s1 = algo.Node(start_point, goal_point, [0, 0], robot_radius + clearance,
rpm[0], rpm[1])
path, explored = s1.astar()
plt.title('Path planning implemented for Turtlebot 3 using A* Algorithm',
fontsize=10)
# Plotting the explored nodes and final path
points1x = []
points1y = []
points2x = []
points2y = []
points3x = []
points3y = []
points4x = []
points4y = []
for point in range(1, len(explored)):
#print('Explored point:', explored[point])
points1x.append(explored[point][4][0])
points1y.append(explored[point][4][1])
points2x.append(explored[point][1][0] - (explored[point][4][0]))
points2y.append(explored[point][1][1] - (explored[point][4][1]))
#plt.quiver(explored[point][4][0], explored[point][4][1], explored[point][1][0]-(explored[point][4][0]), explored[point][1][1]-(explored[point][4][1]), units='xy' ,scale=1, label = 'Final Path', color = 'g', width =0.02, headwidth = 1,headlength=0)
#if point%10 == 0:
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point) + '.png', dpi = 300)
print('Path length:', len(path))
for point in range(1, len(path)):
if point + 1 < len(path):
#odom_sub = rospy.Subscriber('/odom', Odometry, callback)
vel_msg.linear.x = (path[point][5][0]**2 +
path[point][5][1]**2)**(1 / 2)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = path[point][5][2]
velocity_publisher.publish(vel_msg)
print('Point:', point)
now = rospy.get_rostime()
print('ROS Time:', now.secs)
time.sleep(1)
points3x.append(path[point][0])
points3y.append((path[point][1]))
points4x.append((path[point + 1][0]) - (path[point][0]))
points4y.append((path[point + 1][1]) - (path[point][1]))
#plt.quiver(path[point][0], (path[point][1]), (path[point+1][0])-(path[point][0]), (path[point+1][1])-(path[point][1]), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point+len(explored)) + '.png', dpi = 300)
else:
vel_msg.linear.x = (path[point][5][0]**2 +
path[point][5][1]**2)**(1 / 2)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = path[point][5][2]
velocity_publisher.publish(vel_msg)
points3x.append(path[point][0])
points3y.append((path[point][1]))
points4x.append((path[-1][0]) - (path[point][0]))
points4y.append((path[-1][1]) - (path[point][1]))
#plt.quiver(path[point][0], (path[point][1]), (path[-1][0])-(path[point][0]), (path[-1][1])-(path[point][1]), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point+len(explored)) + '.png', dpi = 300)
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
velocity_publisher.publish(vel_msg)
plt.quiver(np.array(points1x),
np.array(points1y),
np.array(points2x),
np.array(points2y),
units='xy',
scale=1,
label='Final Path',
color='g',
width=0.02,
headwidth=1,
headlength=0)
plt.quiver(np.array(points3x),
np.array(points3y),
np.array(points4x),
np.array(points4y),
units='xy',
scale=1,
label='Final Path',
color='b',
width=0.02,
headwidth=1,
headlength=0)
plt.show()
plt.close()
def rrt(start, goal, clearance):
cost2come1 = 0
nodes1 = []
start.append(cost2come1)
start1 = tuple(start)
explored_nodes1[start1] = start1
nodes1.append(start1)
cost2come2 = 0
nodes2 = []
goal.append(cost2come2)
start2 = tuple(goal)
explored_nodes2[start2] = start2
nodes2.append(start2)
# Running the algo for 'NUMNODES' number of iterations
for i in range(NUMNODES):
# Tree generation from start node
rand1 = random.random() * (XDIM / 2) * random.choice(
plusminus), random.random() * (YDIM / 2) * random.choice(plusminus)
while not utils.check_node(rand1, clearance):
rand1 = random.random() * (
XDIM / 2) * random.choice(plusminus), random.random() * (
YDIM / 2) * random.choice(plusminus)
nearest_node1 = nodes1[0]
for p1 in nodes1:
if dist(p1, rand1) < dist(nearest_node1, rand1):
nearest_node1 = p1
new_node1 = interpolate(nearest_node1, rand1, nearest_node1[3])
#print('New Node1:', new_node1)
#print('New node:', new_node1)
new_node1, nearest_node1 = choose_parent(nearest_node1, new_node1,
nodes1)
if utils.check_node(new_node1, clearance):
explored_nodes1[new_node1] = nearest_node1
nodes1.append(new_node1)
# Check if the new node from tree 1 can be joined to tree 2
nearest_node3 = nodes2[0]
for p in nodes2:
if dist(p, new_node1) < dist(nearest_node3, new_node1):
nearest_node3 = p
if valid_joint(new_node1, nearest_node3, clearance):
print('Joint from tree 1 to tree 2 found!!')
explored_nodes1[nearest_node3] = new_node1
final_path1 = back_track1(nearest_node3, start1)
final_path2 = back_track2(nearest_node3, start2)
return explored_nodes1, explored_nodes2, final_path1, final_path2
else:
# Tree generation from goal node
rand2 = random.random() * (
XDIM / 2) * random.choice(plusminus), random.random() * (
YDIM / 2) * random.choice(plusminus)
while not utils.check_node(rand2, clearance):
rand2 = random.random() * (
XDIM / 2) * random.choice(plusminus), random.random() * (
YDIM / 2) * random.choice(plusminus)
nearest_node2 = nodes2[0]
for p2 in nodes2:
if dist(p2, rand2) < dist(nearest_node2, rand2):
nearest_node2 = p2
new_node2 = interpolate(nearest_node2, rand2, nearest_node2[3])
#print('New Node2:', new_node2)
new_node2, nearest_node2 = choose_parent(nearest_node2, new_node2,
nodes2)
if utils.check_node(new_node2, clearance):
explored_nodes2[new_node2] = nearest_node2
nodes2.append(new_node2)
# Again check if the new node from tree 1 can be joined to tree 2
nearest_node3 = nodes2[0]
for p in nodes2:
if dist(p, new_node1) < dist(nearest_node3, new_node1):
nearest_node3 = p
if valid_joint(new_node1, nearest_node3, clearance):
print('Joint from tree 1 to tree 2 found!!')
explored_nodes2[nearest_node3] = new_node1
final_path1 = back_track1(nearest_node3, start1)
final_path2 = back_track2(nearest_node3, start2)
return explored_nodes1, explored_nodes2, final_path1, final_path2
return explored_nodes1, explored_nodes2
def main():
# Taking inputs from the user
clearance = eval(input('Please enter the clearance value of the robot from the obstacle:'))
print('The clearance value you entered is:', clearance)
print('')
start_point = eval(input('Please enter the start coordinates for the robot in this format - [X_coord, Y_coord, Theta]:'))
while not utils.check_node(start_point, clearance):
start_point = eval(input('Please enter the start coordinates in this format - [X_coord, Y_coord, Theta]:'))
start_circle = plt.scatter(start_point[0], start_point[1], c = 'b')
print('The start point you entered is:', start_point)
print('')
goal_point = eval(input('Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord]:'))
while not utils.check_node(goal_point, clearance):
goal_point = eval(input('Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord]:'))
goal_circle = plt.scatter(goal_point[0], goal_point[1], c = 'y')
print('The goal point you entered is:', goal_point)
print('')
goal_circle = plt.Circle((goal_point[0], goal_point[1]), radius= 0.25,fill=False)
plt.gca().add_patch(goal_circle)
rpm = eval(input('Please enter the RPM for both the wheels in this format - [RPM1,RPM2]:'))
print("The wheel RPM's you entered for both the wheels are:", rpm)
print('')
robot_radius = 0.089
s1 = algo.Node(start_point, goal_point, [0,0], robot_radius+clearance, rpm[0], rpm[1])
path, explored = s1.astar()
plt.title('Path planning implemented for Turtlebot 3 using A* Algorithm',fontsize=10)
# Plotting the explored nodes and final path
points1x = []
points1y = []
points2x = []
points2y = []
points3x = []
points3y = []
points4x = []
points4y = []
for point in range(1,len(explored)):
#print('Explored point:', explored[point])
points1x.append(explored[point][4][0])
points1y.append(explored[point][4][1])
points2x.append(explored[point][1][0]-(explored[point][4][0]))
points2y.append(explored[point][1][1]-(explored[point][4][1]))
#plt.quiver(explored[point][4][0], explored[point][4][1], explored[point][1][0]-(explored[point][4][0]), explored[point][1][1]-(explored[point][4][1]), units='xy' ,scale=1, label = 'Final Path', color = 'g', width =0.02, headwidth = 1,headlength=0)
#if point%10 == 0:
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point) + '.png', dpi = 300)
for point in range(len(path)):
if point+1 < len(path):
points3x.append(path[point][0])
points3y.append((path[point][1]))
points4x.append((path[point+1][0])-(path[point][0]))
points4y.append((path[point+1][1])-(path[point][1]))
#plt.quiver(path[point][0], (path[point][1]), (path[point+1][0])-(path[point][0]), (path[point+1][1])-(path[point][1]), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point+len(explored)) + '.png', dpi = 300)
else:
points3x.append(path[point][0])
points3y.append((path[point][1]))
points4x.append((path[-1][0])-(path[point][0]))
points4y.append((path[-1][1])-(path[point][1]))
#plt.quiver(path[point][0], (path[point][1]), (path[-1][0])-(path[point][0]), (path[-1][1])-(path[point][1]), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
#plt.savefig('/home/nalindas9/Desktop/images/'+'img' + str(point+len(explored)) + '.png', dpi = 300)
plt.quiver(np.array(points1x), np.array(points1y), np.array(points2x), np.array(points2y), units='xy' ,scale=1, label = 'Final Path', color = 'g', width =0.02, headwidth = 1,headlength=0)
plt.quiver(np.array(points3x), np.array(points3y), np.array(points4x), np.array(points4y), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
plt.show()
plt.close()
def main():
# Taking inputs from the user
clearance = eval(
input(
'Please enter the clearance value of the robot from the obstacle:')
)
print('The clearance value you entered is:', clearance)
print('')
start_point = eval(
input(
'Please enter the start coordinates for the robot in this format - [X_coord, Y_coord, Theta]:'
))
while not utils.check_node(start_point, clearance):
start_point = eval(
input(
'Please enter the start coordinates in this format - [X_coord, Y_coord, Theta]:'
))
start_circle = plt.scatter(start_point[0], start_point[1], c='b')
print('The start point you entered is:', start_point)
print('')
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord, Theta]:'
))
while not utils.check_node(goal_point, clearance):
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord, Theta]:'
))
goal_circle = plt.scatter(goal_point[0], goal_point[1], c='y')
print('The goal point you entered is:', goal_point)
print('')
goal_circle = plt.Circle((goal_point[0], goal_point[1]),
radius=0.25,
fill=False)
plt.gca().add_patch(goal_circle)
rpm = eval(
input(
'Please enter the RPM for both the wheels in this format - [RPM1,RPM2]:'
))
print("The wheel RPM's you entered for both the wheels are:", rpm)
print('')
explored1, explored2, final_path1, final_path2 = algo.rrt(
start_point, goal_point, clearance)
final_path2.pop(-1)
final_path2.reverse()
final_path1_spline = copy.deepcopy(final_path1)
final_path1_spline.pop(-1)
spline_pts1 = utils.cubic_spline(final_path1_spline)
spline_pts2 = utils.cubic_spline(final_path2)
final_path = final_path1 + final_path2
final_spline_path = spline_pts1 + spline_pts2
print('The final path is:', final_spline_path)
# Plotting the explored nodes and final path
points1x = []
points1y = []
points2x = []
points2y = []
points3x = []
points3y = []
points4x = []
points4y = []
points5x = []
points5y = []
points6x = []
points6y = []
points7x = []
points7y = []
points8x = []
points8y = []
for point in explored1.keys():
points1x.append(point[0])
points1y.append(point[1])
points2x.append(explored1[point][0] - point[0])
points2y.append(explored1[point][1] - point[1])
for point in explored2.keys():
points3x.append(point[0])
points3y.append(point[1])
points4x.append(explored2[point][0] - point[0])
points4y.append(explored2[point][1] - point[1])
for point in range(len(final_path)):
if point + 1 < len(final_path):
points5x.append(final_path[point][0])
points5y.append((final_path[point][1]))
points6x.append((final_path[point + 1][0]) -
(final_path[point][0]))
points6y.append((final_path[point + 1][1]) -
(final_path[point][1]))
else:
points5x.append(final_path[point][0])
points5y.append((final_path[point][1]))
points6x.append((final_path[-1][0]) - (final_path[point][0]))
points6y.append((final_path[-1][1]) - (final_path[point][1]))
"""
for point in range(len(final_path2)):
if point+1 < len(final_path2):
points7x.append(final_path2[point][0])
points7y.append((final_path2[point][1]))
points8x.append((final_path2[point+1][0])-(final_path2[point][0]))
points8y.append((final_path2[point+1][1])-(final_path2[point][1]))
else:
points7x.append(final_path2[point][0])
points7y.append((final_path2[point][1]))
points8x.append((final_path2[-1][0])-(final_path2[point][0]))
points8y.append((final_path2[-1][1])-(final_path2[point][1]))
"""
plt.quiver(np.array(points1x),
np.array(points1y),
np.array(points2x),
np.array(points2y),
units='xy',
scale=1,
label='Explored nodes',
color='g',
width=0.02,
headwidth=1,
headlength=0)
plt.quiver(np.array(points3x),
np.array(points3y),
np.array(points4x),
np.array(points4y),
units='xy',
scale=1,
label='Explored nodes',
color='g',
width=0.02,
headwidth=1,
headlength=0)
plt.quiver(np.array(points5x),
np.array(points5y),
np.array(points6x),
np.array(points6y),
units='xy',
scale=1,
label='Final Path',
width=0.07,
headwidth=1,
headlength=0)
#plt.quiver(np.array(points7x), np.array(points7y), np.array(points8x), np.array(points8y), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
plt.show()
plt.close()
def main():
rospy.init_node('Adrurlbot_vel_publisher', anonymous=True)
r = rospy.Rate(4)
def odom_callback(msg):
global x
global y
global theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
#(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
(roll, pitch,
theta) = utils.quaternion_to_euler(rot_q.w, rot_q.x, rot_q.y, rot_q.z)
sub = rospy.Subscriber("/odom", Odometry, odom_callback)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
vel_msg = Twist()
# Taking inputs from the user
time.sleep(2)
print('')
print(
"************************* Let's move the turtlebot! *******************************"
)
print('')
clearance = eval(
input(
'Please enter the clearance value of the robot from the obstacle:')
)
print('The clearance value you entered is:', clearance)
print('')
start_point = eval(
input(
'Please enter the start coordinates for the robot in this format - [X_coord, Y_coord, Theta]:'
))
while not utils.check_node(start_point, clearance):
start_point = eval(
input(
'Please enter the start coordinates in this format - [X_coord, Y_coord, Theta]:'
))
start_circle = plt.scatter(start_point[0], start_point[1], c='b')
print('The start point you entered is:', start_point)
print('')
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord, Theta]:'
))
while not utils.check_node(goal_point, clearance):
goal_point = eval(
input(
'Please enter the goal coordinates of the robot in this format - [X_coord, Y_coord, Theta]:'
))
goal_circle = plt.scatter(goal_point[0], goal_point[1], c='y')
print('The goal point you entered is:', goal_point)
print('')
goal_circle = plt.Circle((goal_point[0], goal_point[1]),
radius=0.25,
fill=False)
plt.gca().add_patch(goal_circle)
explored1, explored2, final_path1, final_path2 = algo.rrt(
start_point, goal_point, clearance)
final_path2.pop(-1)
final_path2.reverse()
final_path1_spline = copy.deepcopy(final_path1)
final_path1_spline.pop(-1)
spline_pts1 = utils.cubic_spline(final_path1_spline)
spline_pts2 = utils.cubic_spline(final_path2)
final_path = final_path1 + final_path2
final_spline_path = spline_pts1 + spline_pts2
print('The final path is:', final_path)
goal = Point()
for points in final_spline_path:
goal.x = points[0]
goal.y = points[1]
print('Point:', (points[0], points[1]))
while not rospy.is_shutdown():
inc_x = goal.x - x
inc_y = goal.y - y
angle_to_goal = atan2(inc_y, inc_x)
print('Angle diff:', angle_to_goal - theta)
if angle_to_goal - theta > 0:
if abs(angle_to_goal - theta) > 0.05:
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.1
else:
vel_msg.linear.x = 0.2
vel_msg.angular.z = 0.0
else:
if abs(angle_to_goal - theta) > 0.05:
vel_msg.linear.x = 0.0
vel_msg.angular.z = -0.1
else:
vel_msg.linear.x = 0.2
vel_msg.angular.z = 0.0
#print('Distance to next point:', math.sqrt((inc_x)**2 + (inc_y)**2))
if math.sqrt((inc_x)**2 + (inc_y)**2) < 0.03:
break
pub.publish(vel_msg)
r.sleep()
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.0
pub.publish(vel_msg)
# Plotting the explored nodes and final path
points1x = []
points1y = []
points2x = []
points2y = []
points3x = []
points3y = []
points4x = []
points4y = []
points5x = []
points5y = []
points6x = []
points6y = []
points7x = []
points7y = []
points8x = []
points8y = []
for point in explored1.keys():
points1x.append(point[0])
points1y.append(point[1])
points2x.append(explored1[point][0] - point[0])
points2y.append(explored1[point][1] - point[1])
for point in explored2.keys():
points3x.append(point[0])
points3y.append(point[1])
points4x.append(explored2[point][0] - point[0])
points4y.append(explored2[point][1] - point[1])
for point in range(len(final_path)):
if point + 1 < len(final_path):
points5x.append(final_path[point][0])
points5y.append((final_path[point][1]))
points6x.append((final_path[point + 1][0]) -
(final_path[point][0]))
points6y.append((final_path[point + 1][1]) -
(final_path[point][1]))
else:
points5x.append(final_path[point][0])
points5y.append((final_path[point][1]))
points6x.append((final_path[-1][0]) - (final_path[point][0]))
points6y.append((final_path[-1][1]) - (final_path[point][1]))
"""
for point in range(len(final_path2)):
if point+1 < len(final_path2):
points7x.append(final_path2[point][0])
points7y.append((final_path2[point][1]))
points8x.append((final_path2[point+1][0])-(final_path2[point][0]))
points8y.append((final_path2[point+1][1])-(final_path2[point][1]))
else:
points7x.append(final_path2[point][0])
points7y.append((final_path2[point][1]))
points8x.append((final_path2[-1][0])-(final_path2[point][0]))
points8y.append((final_path2[-1][1])-(final_path2[point][1]))
"""
plt.quiver(np.array(points1x),
np.array(points1y),
np.array(points2x),
np.array(points2y),
units='xy',
scale=1,
label='Explored nodes',
color='g',
width=0.02,
headwidth=1,
headlength=0)
plt.quiver(np.array(points3x),
np.array(points3y),
np.array(points4x),
np.array(points4y),
units='xy',
scale=1,
label='Explored nodes',
color='g',
width=0.02,
headwidth=1,
headlength=0)
plt.quiver(np.array(points5x),
np.array(points5y),
np.array(points6x),
np.array(points6y),
units='xy',
scale=1,
label='Final Path',
width=0.07,
headwidth=1,
headlength=0)
#plt.quiver(np.array(points7x), np.array(points7y), np.array(points8x), np.array(points8y), units='xy' ,scale=1, label = 'Final Path', width =0.07, headwidth = 1,headlength=0)
plt.show()
plt.close()
