def fourthAction(node,angle,stepSize,goal):
# print("FOURTH ACTION:::::::::::::::::::::::")
newNode = st.Node(0, 0, math.inf, math.inf, math.inf, (0, 0),math.inf,math.inf,math.inf,math.inf)
newNode.x = node.x + stepSize * np.cos(np.deg2rad(angle - 30))
newNode.y = node.y + stepSize * np.sin(np.deg2rad(angle - 30))
newNode.angle = angle - 30
if newNode.angle < 0:
newNode.angle = newNode.angle + 360
else:
newNode.angle = newNode.angle
if newNode.x < 0 or newNode.x > 300 or newNode.y < 0 or newNode.y > 200 :
return False, node
# if check_Obstacle(node.x, node.y):
# return False, node
if isNodeVisited(newNode)== False:
return False, node
else:
newNode.cTc = node.cTc + stepSize
newNode.cTg = calcEuclideanDist(newNode.x,newNode.y,goal)
newNode.totalCost = newNode.cTc + newNode.cTg
newNode.parent = (node.x,node.y)
return True, newNode
def Action(ur, ul, node, goal):
t = 0
dt = 0.1
r = 0.038
L = 0.3175
dx = node.x
dy = node.y
d_theta = np.deg2rad(node.angle)
while t < 1:
t = t + dt
dx += (r / 2) * (ul + ur) * np.cos(d_theta) * dt
dy += (r / 2) * (ul + ur) * np.sin(d_theta) * dt
d_theta += (r / L) * (ur - ul) * dt
if check_Obstacle(dx, dy):
return False, node
d_theta = np.rad2deg(d_theta)
newNode = st.Node(0, 0, math.inf, math.inf, math.inf, (0, 0), math.inf,
math.inf, math.inf, math.inf)
newNode.x = dx
newNode.y = dy
newNode.angle = d_theta
newNode.RPM = (ul, ur)
for i in range(1, 13):
if newNode.angle > 360 * i:
newNode.angle = newNode.angle - 360 * (i + 1)
elif newNode.angle < -360 * i:
newNode.angle = newNode.angle + 360 * (i + 1)
elif -360 < newNode.angle < 0:
newNode.angle = newNode.angle + 360
else:
newNode.angle = newNode.angle
if newNode.x < -5 or newNode.x > 5 or newNode.y < -5 or newNode.y > 5:
return False, node
if isNodeVisited(newNode) == False:
return False, node
else:
newNode.cTc = node.cTc + np.sqrt(
np.power(node.x - dx, 2) + np.power(node.y - dy, 2))
newNode.cTg = calcEuclideanDist(newNode.x, newNode.y, goal)
newNode.totalCost = newNode.cTc + newNode.cTg
newNode.parent = (node.x, node.y)
newNode.parentAngle = node.angle
return True, newNode
step_flag = True
continue
angle = int(input("Enter the start angle in degrees: "))
if angle <-360 or angle >360:
print("Angle Out of Bounds!!")
angle_flag = True
continue
else:
break
except:
if step_flag is True:
print("Enter a valid integer in range 1-10.")
if angle_flag is True:
print("Enter a valid angle in range -360 to 360 degrees")
node = st.Node(int(start[0]), int(start[1]), calcEuclideanDist(start[0],start[1],goal),0,calcEuclideanDist(start[0],start[1],goal), (0, 0),angle, 0 , 0 , 0)
start_time = time.time()
# Check if the input goal lies within the obstacle space.
if check_Obstacle(goal[0], goal[1]):
print("Goal cannot be reached")
else:
# aStar(node, goal)
try:
print("Exploring nodes...")
ExploredNodeList =aStar(node, goal)
except:
# Print the following if the goal cannot be reached since no path is available
print("The radius and clearance is too Big!! Goal cannot be reached !!")