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')