def mapcallback(mdata): global _way_points # convert occupancy grid to numpy data = np.asarray(mdata.data, dtype=np.int8).reshape(mdata.info.height, mdata.info.width) data[data != 0] = 255 raw_img = Image.fromarray(data, 'L') raw_img.save(dir_path + "/raw_map.png", "PNG") # inflate the map inflate_size = int(math.ceil(robot_size / map_resolution)) + 1 inflated_data = inflate_map(np.copy(data), inflate_size) inflated_img = Image.fromarray(inflated_data, 'L') inflated_img.save(dir_path + "/inflated_map.png", "PNG") # do path planning using A* algorithm & save gmap = OccupancyGridMap(inflated_data, cell_size=map_resolution, occupancy_threshold=1) path, path_idx = a_star_occupancy(starting_location, goal_location, gmap, movement='8N') if not path: rospy.logerr("Goal is not reachable") _way_points = deque(path) print "way points: ", _way_points print "navigation plan idx: \n", path_idx with open(dir_path + '/navigation_plan.txt', 'w') as f: for waypoint in path_idx: line = ' '.join(str(ele) for ele in waypoint) f.write(line + '\n') rospy.loginfo("success")
def plot_path(frame, start_node, end_node): global CNT # convert to png cv2.imwrite("input_frame%d.png" % CNT, frame) # plot path gmap_splash = OccupancyGridMap.from_png("input_frame%d.png" % CNT, 1, splash=True) gmap_normal = OccupancyGridMap.from_png("input_frame%d.png" % CNT, 1) process_path(gmap_splash, start_node, end_node, True) process_path(gmap_normal, start_node, end_node) CNT += 1
def a_star_function(X, Y, taskX, taskY): # load the map gmap = OccupancyGridMap.from_png('among-us-edges-fixed-ai1.png', .05) # set a start and an end node (in meters) start_node = (X, Y) ##AMONG US: Where we are goal_node = (taskX, taskY) ##AMONG US: Where the task is :) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='4N') gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else: print('Goal is not reachable') # plot start and goal points over the map (in pixels) start_node_px = gmap.get_index_from_coordinates( start_node[0], start_node[1]) goal_node_px = gmap.get_index_from_coordinates(goal_node[0], goal_node[1]) plt.plot(start_node_px[0], start_node_px[1], 'ro') plt.plot(goal_node_px[0], goal_node_px[1], 'go') plt.show()
from .. import gridmap from gridmap import OccupancyGridMap import matplotlib.pyplot as plt from .. import a_star from a_star import a_star from .. import utils from utils import plot_path if __name__ == '__main__': # load the map gmap = OccupancyGridMap.from_png( '~/ros_workspaces/project/src/stdr_simulator/stdr_resources/maps/among-us-edges-map.png', 1) # set a start and an end node (in meters) start_node = (12, 9) ##AMONG US: Where we are goal_node = (8, 5) ##AMONG US: Where the task is :) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='8N') gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else: print('Goal is not reachable') # plot start and goal points over the map (in pixels) start_node_px = gmap.get_index_from_coordinates(
from gridmap import OccupancyGridMap import matplotlib.pyplot as plt from a_star import a_star from utils import plot_path if __name__ == '__main__': # load the map gmap = OccupancyGridMap.from_png('maps/example_map_binary.png', 1) # set a start and an end node (in meters) start_node = (360.0, 330.0) goal_node = (285.0, 86.0) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='4N') gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else: print('Goal is not reachable') # plot start and goal points over the map (in pixels) start_node_px = gmap.get_index_from_coordinates( start_node[0], start_node[1]) goal_node_px = gmap.get_index_from_coordinates(goal_node[0], goal_node[1]) plt.plot(start_node_px[0], start_node_px[1], 'ro')
import sys import matplotlib.pyplot as plt parent_folder = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..') if parent_folder not in sys.path: sys.path.append(parent_folder) from gridmap import OccupancyGridMap import matplotlib.pyplot as plt from a_star import a_star from utils import plot_path if __name__ == '__main__': # load the map #gmap = OccupancyGridMap.from_png('maps/example_map_occupancy.png', 1) gmap = OccupancyGridMap.from_png('maps/sample_map.png', 1) # set a start and an end node (in meters) start_node = (260.0, 400.0) goal_node = (540.0, 170.0) #goal_node = (400.0, 170.0) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='8N') gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else:
def a_star_function(X, Y, taskX, taskY, robotName): timeout = 100 time1 = time.time() gmap = rospy.wait_for_message("map/occupancy_grid", OccupancyGridUpdate, timeout) #cell_size = gmap.resolution cell_size = .25 ## (7, 12.5) ### Need to fix how it's being reshaped warnings.filterwarnings("ignore") data_array = np.reshape(gmap.occupancy_grid, (-1, gmap.height)) data_array = np.fliplr(np.rot90(data_array, -1)) gmap = OccupancyGridMap(data_array, cell_size) #OccupancyGridMap.from_png('among-us-edges-fixed-ai1.png', .05) # set a start and an end node (in meters) start_node = (X, Y) ##AMONG US: Where we are goal_node = (taskX, taskY) ##AMONG US: Where the task is :) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='4N') ''' gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else: print('Goal is not reachable') # plot start and goal points over the map (in pixels) start_node_px = gmap.get_index_from_coordinates(start_node[0], start_node[1]) goal_node_px = gmap.get_index_from_coordinates(goal_node[0], goal_node[1]) plt.plot(start_node_px[0], start_node_px[1], 'ro') plt.plot(goal_node_px[0], goal_node_px[1], 'go') plt.show() ''' rospy.set_param(robotName + '/path', tupleListToString(path)) path = path_cleaner(path) time2 = time.time() print('A Star Time:' + str(time2 - time1)) return path
for i in range(1, len(array) - 1): waypoint = array[i] if (waypoint[0] == array[i - 1][0]): if (waypoint[0] != array[i + 1][0]): path.append(waypoint) if (waypoint[1] == array[i - 1][1]): if (waypoint[1] != array[i + 1][1]): path.append(waypoint) path.append(array[len(array) - 1]) return path if __name__ == '__main__': # load the map gmap = OccupancyGridMap.from_png('among-us-edges-fixed-ai1.png', .05) # set a start and an end node (in meters) start_node = (15, 12) ##AMONG US: Where we are goal_node = (22, 7) ##AMONG US: Where the task is :) # run A* path, path_px = a_star(start_node, goal_node, gmap, movement='4N') gmap.plot() if path: # plot resulting path in pixels over the map plot_path(path_px) else: