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: