def main(): # Init ROS node rospy.init_node('task', anonymous=True) # Create subscriber for position, goal, boost points, and obstacles rospy.Subscriber('/goal', Pose, goal_callback) rospy.Subscriber('/boost', PoseArray, boost_callback) rospy.Subscriber("/dynamic_obstacles", PoseArray, dynamic_obstacles_callback) # Wait for resources to become active goal = rospy.wait_for_message("/goal", Pose).position boosts = rospy.wait_for_message("/boost", PoseArray).poses obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses # Create map service client getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap) rospy.wait_for_service('/GlobalMap') try: raw_map = getMap() except rospy.ServiceException as e: print("Map service error: " + str(e)) return # Get map as 2D list world_map = util.parse_map(raw_map) shortest_path = bfs.bfs(world_map, (0, 0), (goal.y, goal.x)) # Print resources print("Wall layout:") util.print_map(world_map) print("Boost points:") util.print_positions(boosts) print("Obstacles at start:") util.print_positions(obstacles) # Initialize drone drone = Drone() drone.takeoff() # -- For example code -- target_x = 0 target_y = 0 drone.set_target(target_x, target_y) rate = rospy.Rate(30) print('there is a test') i = 0 targets = [[0, 0], [1, 0], [1, 7], [2, 7], [16, 7], [17, 7], [19, 7], [19, 20]] for t in targets: goto(t[0], t[1], rospy, rate, drone)
def main(): # Init ROS node rospy.init_node('task', anonymous=True) # Create subscriber for position, goal, boost points, and obstacles rospy.Subscriber('/goal', Pose, goal_callback) rospy.Subscriber('/boost', PoseArray, boost_callback) rospy.Subscriber("/dynamic_obstacles", PoseArray, dynamic_obstacles_callback) # Wait for resources to become active goal = rospy.wait_for_message("/goal", Pose).position boosts = rospy.wait_for_message("/boost", PoseArray).poses obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses # Create map service client getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap) rospy.wait_for_service('/GlobalMap') try: raw_map = getMap() except rospy.ServiceException as e: print("Map service error: " + str(e)) return # Get map as 2D list world_map = util.parse_map(raw_map) # Print resources print("Wall layout:") util.print_map(world_map) print("Boost points:") util.print_positions(boosts) print("Obstacles at start:") util.print_positions(obstacles) # Initialize drone drone = Drone() drone.takeoff() # -- For example code -- target_x = 0 target_y = 0 coords = create_waypoints() i = 0 rate = rospy.Rate(30) while not rospy.is_shutdown(): rate.sleep() # ------------------------------- # ------Replace this example----- # ---------with your code!------- # ------------------------------- # Find how far we are away from target if i == 0: drone.set_target(0, 0) distance_to_target = ((target_x - drone.position.x)**2 + (target_y - drone.position.y)**2)**0.5 # Do special action if we are close if distance_to_target < 0.5: if i < len(coords) - 1: i = i + 1 print("Next index is ", i) else: drone.takeoff(height=0) target_x = coords[i][1] target_y = coords[i][0] # Move to random point drone.set_target(target_x, target_y) else: print("distance to target:", distance_to_target)
def main(): # Init ROS node rospy.init_node('task', anonymous=True) # Create subscriber for position, goal, boost points, and obstacles rospy.Subscriber('/goal', Pose, goal_callback) rospy.Subscriber('/boost', PoseArray, boost_callback) rospy.Subscriber("/dynamic_obstacles", PoseArray, dynamic_obstacles_callback) # Wait for resources to become active goal = rospy.wait_for_message("/goal", Pose).position boosts = rospy.wait_for_message("/boost", PoseArray).poses obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses print('Goal is at {}'.format(goal)) # Create map service client getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap) rospy.wait_for_service('/GlobalMap') try: raw_map = getMap() except rospy.ServiceException as e: print("Map service error: " + str(e)) return # Get map as 2D list world_map = util.parse_map(raw_map) with open('static_map_gen.txt') as f: f.writelines('\t'.join(str(j) for j in i) + '\n' for i in world_map) # Print resources print("Wall layout:") util.print_map(world_map) print("Boost points:") util.print_positions(boosts) print("Obstacles at start:") util.print_positions(obstacles) path = make_abs_path( compress_path(make_rel_path(find_path(world_map, 0, 0, goal.x, goal.y)))) # Initialize drone drone = Drone() drone.activate() drone.takeoff() (target_y, target_x) = (0, 0) drone.set_target(target_x, target_y) rate = rospy.Rate(30) pathidx = 0 while not rospy.is_shutdown(): rate.sleep() distance_to_target = ((target_x - drone.position.x)**2 + (target_y - drone.position.y)**2)**0.5 # Do special action if we are close if distance_to_target < 0.5: # Print current distance to goal. Note that we # wont reach the goal, since we just move randomly distance_to_goal = ((drone.position.x - goal.x)**2 + (drone.position.y - goal.y)**2)**0.5 print("Distance to goal is now", distance_to_goal) # Get next target (target_y, target_x) = path[path_idx] print("New target", target_x, target_y) # Move to target drone.set_target(target_x, target_y) path_idx = path_idx + 1 if (path_idx >= len(path)): print("Can't happen!") pass else: print("distance to target:", distance_to_target)
def main(): # Init ROS node rospy.init_node('task', anonymous=True) # Create subscriber for position, goal, boost points, and obstacles rospy.Subscriber('/goal', Pose, goal_callback) rospy.Subscriber('/boost', PoseArray, boost_callback) rospy.Subscriber("/dynamic_obstacles", PoseArray, dynamic_obstacles_callback) # Wait for resources to become active goal = rospy.wait_for_message("/goal", Pose).position print('Goal is at {}'.format(goal)) boosts = rospy.wait_for_message("/boost", PoseArray).poses obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses # Create map service client getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap) rospy.wait_for_service('/GlobalMap') try: raw_map = getMap() except rospy.ServiceException as e: print("Map service error: " + str(e)) return # Get map as 2D list world_map = util.parse_map(raw_map) with open('dynnamic_map_gen.txt') as f: f.writelines('\t'.join(str(j) for j in i) + '\n' for i in world_map) # Print resources print("Wall layout:") util.print_map(world_map) print("Boost points:") util.print_positions(boosts) print("Obstacles at start:") util.print_positions(obstacles) # Initialize drone drone = Drone() drone.activate() drone.takeoff() # -- For example code -- target_x = 0 target_y = 0 rate = rospy.Rate(30) while not rospy.is_shutdown(): rate.sleep() # ------------------------------- # ------Replace this example----- # ---------with your code!------- # ------------------------------- # Find how far we are away from target distance_to_target = ((target_x - drone.position.x) ** 2 + (target_y - drone.position.y) ** 2) ** 0.5 # Do special action if we are close if distance_to_target < 0.5: # Print current distance to goal. Note that we # wont reach the goal, since we just move randomly distance_to_goal = ((drone.position.x - goal.x) ** 2 + (drone.position.y - goal.y) ** 2) ** 0.5 print("Distance to goal is now", distance_to_goal) # Generate some random point and rotation target_x = random.randint(-3, 3) target_y = random.randint(-3, 3) # Move to random point drone.set_target(target_x, target_y) else: print("distance to target:", distance_to_target)