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)