def pathAcquisition(data):
global currentPath
global size
currentPath = path_points()
currentPath.x = data.x
currentPath.y = data.y
currentPath.yaw = data.yaw
currentPath.dOne = data.dOne
currentPath.dTwo = data.dTwo
size = len(currentPath.x)
def main():
print("\n \nStart!!")
rospy.init_node('a_star_test', anonymous=True, disable_signals=True)
xmin = round(min(ox))
xmax = round(max(ox))
ymin = round(min(oy))
ymax = round(max(oy))
print("X input range: ["),
print(xmin),
print(" --> "),
print(xmax),
print("]")
print("Y input range: ["),
print(ymin),
print(" --> "),
print(ymax),
print("]\n")
dittoS = str(
input("please enter the number of the robot you want to start from: "))
dittoG = str(
input("please enter the number of the robot you want to attach to: "))
rospy.Subscriber("ditto" + dittoS + "/odom", Odometry, callbackS)
rospy.Subscriber("ditto" + dittoG + "/odom", Odometry, callbackG)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if (sx == 0.0 and sy == 0.0 and syaw == 0.0):
rospy.wait_for_message("ditto" + dittoS + "/odom",
Odometry,
timeout=10)
print("Current X Position: "),
print(sx)
print("Current Y Position: "),
print(sy)
print("Current Yaw: "),
print(syaw)
print(" ")
grid_size = input("Please Enter Grid(Plan) Resolution [0->1] : ")
#grid_size = 0.05
#gx = input("Please Enter X Goal Position : ")
#gy = input("Please Enter Y Goal Position : ")
#gtheta = input("Please Enter Yaw Goal : ")
if (gx == 0.0 and gy == 0.0 and gtheta == 0.0):
rospy.wait_for_message("ditto" + dittoG + "/odom",
Odometry,
timeout=10)
print("The Goal Pose You Requested is :"),
print(gx, gy, gtheta)
print(" ")
print("Prepping Planner...")
print(" ")
# if show_animation: # pragma: no cover
# plt.plot(ox, oy, ".k")
# plt.plot(sx, sy, "og")
# plt.plot(gx, gy, "xb")
# plt.grid(True)
# plt.axis("equal")
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
rx.reverse()
ry.reverse()
rx_round = [round(num, 2) for num in rx]
ry_round = [round(num, 2) for num in ry]
print("rx: "),
print(rx_round)
print("ry: "),
print(ry_round)
# listLen = len(rx)
path = path_points()
path.x = rx
path.y = ry
path.yaw = gtheta
path.dOne = dittoS
path.dTwo = dittoG
# path_pub.publish(path)
if show_animation: # pragma: no cover
plt.figure(num="Planned Path")
plt.grid(True)
plt.xlabel("X-Position")
plt.ylabel("Y-Position")
# plt.axis("equal")
plt.plot(rx, ry, "-r")
plt.autoscale(enable=True, axis='both')
plt.show()
# plt.close(1)
answer = None
while answer not in ("yes", "no"):
answer = raw_input(
"Do you want to edit the plan?,Answer with 'yes' or 'no': ")
if answer == "yes":
#plt.close('all')
print(" ")
elif answer == "no":
while True:
path_pub.publish(path)
rate.sleep()
else:
print("Please enter 'yes' or 'no'.")
def main(grid_size):
print("\n \nStart!!")
rospy.init_node('a_star_test', anonymous=True, disable_signals=True)
xmin = round(min(ox))
xmax = round(max(ox))
ymin = round(min(oy))
ymax = round(max(oy))
print("X input range: ["),
print(xmin),
print(" --> "),
print(xmax),
print("]")
print("Y input range: ["),
print(ymin),
print(" --> "),
print(ymax),
print("]\n")
dittoS = str(
raw_input("please enter the number of the robot you want to start from: "))
dittoG = str(
raw_input("please enter the number of the robot you want to attach to: "))
# if len(dittoG) > 1:
# Nekteb El Array From Rear To Front Ditto
rospy.Subscriber("ditto" + dittoG[0] + "/odom", Odometry, callbackG)
# else:
# rospy.Subscriber("ditto" + dittoG + "/odom", Odometry, callbackG)
rospy.Subscriber("ditto" + dittoS + "/odom", Odometry, callbackS)
rate = rospy.Rate(15)
global flagoff
while not rospy.is_shutdown():
if (sx == 0.0 and sy == 0.0 and syaw == 0.0):
rospy.wait_for_message(
"ditto" + dittoS + "/odom", Odometry, timeout=10)
sx_round = round(sx, 2)
sy_round = round(sy, 2)
print("Current X Position: "),
print(sx_round)
print("Current Y Position: "),
print(sy_round)
print("Current Yaw: "),
print(syaw)
print(" ")
if (gx == 0.0 and gy == 0.0 and gtheta == 0.0):
rospy.wait_for_message(
"ditto" + dittoG[0] + "/odom", Odometry, timeout=10)
print ("The Goal Pose You Requested is :"),
print(gx, gy, gtheta)
gx_round = round(gx, 2)
gy_round = round(gy, 2)
print("sx_round:: ", str(sx_round), "sy_round:: ", str(sy_round))
print("gx_round:: ", str(gx_round), "gy_round:: ", str(gy_round))
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx_round, sy_round, "og")
plt.plot(gx_round, gy_round, "xb")
plt.grid(True)
plt.axis("equal")
plt.show()
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
rx.reverse()
ry.reverse()
rx_round = [round(num, 2) for num in rx]
ry_round = [round(num, 2) for num in ry]
print("rx: "),
print(rx_round)
print("ry: "),
print(ry_round)
path = path_points()
path.x = rx_round
path.y = ry_round
path.yaw = gtheta
path.dOne = dittoS
path.dTwo = dittoG
if show_animation: # pragma: no cover
plt.figure(num="Planned Path")
plt.grid(True)
plt.xlabel("X-Position")
plt.ylabel("Y-Position")
plt.plot(rx_round, ry_round, "-r")
plt.autoscale(enable=True, axis='both')
plt.show()
# path_pub.publish(path)
# flagoff==True
# if flagoff==True:
# rate.sleep()
# flagoff=True
# if flagoff==True:
# rospy.signal_shutdown(10)
answer = None
while answer not in ("yes", "no"):
answer = raw_input(
"Do you want to edit the plan?,Answer with 'yes' or 'no': ")
if answer == "yes":
plt.close('all')
#print(" ")
elif answer == "no":
while True:
path_pub.publish(path)
rate.sleep()
else:
print("Please enter 'yes' or 'no'.")
#from mybot_gazebo.msg import PlanarOdometry, PlanarPose, PlanarTwist from gp_abstract_sim.msg import path_points from nav_msgs.msg import Odometry from tf.transformations import euler_from_quaternion # plt.ion() ########################################################################### speed = Twist() current_x = 0.0 current_y = 0.0 current_yaw = 0.0 currentPath = path_points() size = 0 delta_x = 0.0 delta_y = 0.0 angle_to_goal = 0.0 euc = 0.0 new_goal_x = 0.0 new_goal_y = 0.0 new_goal_yaw = 0.0 perf_x = [] perf_y = [] pose_achieved = False
def main(grid_size):
end = ''
print("\n \nStart!!")
rospy.init_node('a_star_test', anonymous=True, disable_signals=True)
# start and goal position
sx = 0 # [m]
sy = 0 # [m]
# gx = -15 # [m]
# gy = 22 # [m]
# grid_size = 1.0 # [m]
robot_radius = 0.25 # [m]
# set obstable positions
ox, oy = [], []
for i in range(-1, 1):
ox.append(i)
oy.append(-1.0)
for i in range(-1, 1):
ox.append(1.0)
oy.append(i)
for i in range(-1, 1):
ox.append(i)
oy.append(1.0)
for i in range(-1, 1):
ox.append(-1.0)
oy.append(i)
xmin = round(min(ox))
xmax = round(max(ox))
ymin = round(min(oy))
ymax = round(max(oy))
print("X input range: ["),
print(xmin),
print(" --> "),
print(xmax),
print("]"),
print("Y input range: ["),
print(ymin),
print(" --> "),
print(ymax),
print("]\n"),
dittoS = str(
input("please enter the number of the robot you want to start from: "))
dittoG = str(
input("please enter the number of the robot you want to attach to: "))
rospy.Subscriber("ditto" + dittoS + "/odom", Odometry, callbackS)
rospy.Subscriber("ditto" + dittoG + "/odom", Odometry, callbackG)
while not rospy.is_shutdown():
if (sx == 0.0 and sy == 0.0 and syaw == 0.0):
rospy.wait_for_message("ditto" + dittoS + "/odom",
Odometry,
timeout=10)
print("Current X Position: "),
print(sx)
print("Current Y Position: "),
print(sy)
print("Current Yaw: "),
print(syaw)
print(" ")
if (gx == 0.0 and gy == 0.0 and gtheta == 0.0):
rospy.wait_for_message("ditto" + dittoG + "/odom",
Odometry,
timeout=10)
print("The Goal Pose You Requested is :"),
print(gx, gy, gtheta)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
print("rx: ", rx)
print("ry: ", ry)
path = path_points()
path.x = rx
path.y = ry
path.yaw = gtheta
path.dOne = dittoS
path.dTwo = dittoG
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
path_pub.publish(path)
flagoff = True
if flagoff == True:
rospy.signal_shutdown('Finish')
