def plan_path(surface, arguments): from planning.planning.global_planning.planning \ import PlanningParameters, \ plan from planning.surface.polygon import Polygon # initial_point_x, initial_point_y = tuple(arguments.start) initial_polygon = \ Polygon( [ (10 * (initial_point_x // 10), 10 * (initial_point_y // 10), 0), (10 * (initial_point_x // 10) + 10, 10 * (initial_point_y // 10), 0), (10 * (initial_point_x // 10) + 10, 10 * (initial_point_y // 10) + 10, 0), (10 * (initial_point_x // 10), 10 * (initial_point_y // 10) + 10, 0), ], 0.0 ) final_point_x, final_point_y = tuple(arguments.goal) final_polygon = \ Polygon( [ (10 * (final_point_x // 10), 10 * (final_point_y // 10), 0), (10 * (final_point_x // 10) + 10, 10 * (final_point_y // 10), 0), (10 * (final_point_x // 10) + 10, 10 * (final_point_y // 10) + 10, 0), (10 * (final_point_x // 10), 10 * (final_point_y // 10) + 10, 0), ], 0.0 ) # Установка параметров планирования пути planning_parameters = PlanningParameters() planning_parameters.surface = surface planning_parameters.planner = "a-star-planner" # planning_parameters.planner = "progressive-a-star-planner" # planning_parameters.planner = "ant-colony-planner" planning_parameters.initial_polygon = initial_polygon #list(surface.polygons)[0] planning_parameters.final_polygon = final_polygon planning_parameters.smoothing_depth = 1 # Планирование пути planning_result = plan(planning_parameters) return planning_result
# Планирование глобального пути
planning_result = plan_path(surface, arguments)
states_sequence, _ = planning_result
for state in states_sequence:
polygon = state.polygons_sequence[-1]
map_polygon_index = surface.get_map_polygon_index(polygon)
arguments.output.write("%s\n" % map_polygon_index)
#!!!!!<Временно>
from planning.planning.local_planning.trajectory_planning import plan
polygons_sequence = \
[state.polygons_sequence[-1] for state in states_sequence]
plan(surface, states_sequence, tuple(arguments.start), tuple(arguments.goal))
#!!!!!</Временно>
# Визуализация
if arguments.visualization is not None:
if not check_module_presence("svgwrite"):
sys.stderr.write(
"Ошибка: для визуализации спланированного пути требуется" \
+ " svgwrite\n"
)
else:
visualize_path(planning_result, surface, arguments)
