94 lines
3.0 KiB
Python
94 lines
3.0 KiB
Python
from typing import Dict, List
|
|
|
|
from shapely.geometry import Polygon, Point
|
|
|
|
from pyrate.plan.geometry.polygon import CartesianPolygon
|
|
from pyrate.plan.nearplanner.cost_functions import *
|
|
from pyrate.plan.nearplanner.evaluated_timing_frame import EvaluatedTimingFrame
|
|
from pyrate.plan.nearplanner.holders import EstimationParameters
|
|
from pyrate.plan.nearplanner.holders import OptimizationParameters
|
|
from pyrate.plan.nearplanner.obstacle import Obstacle
|
|
from pyrate.plan.nearplanner.optimizer import Optimizer
|
|
|
|
estimation_param = EstimationParameters()
|
|
optimization_param = OptimizationParameters(estimation_param)
|
|
|
|
optimization_param.verbose = False
|
|
|
|
|
|
def create_context(
|
|
position: Point,
|
|
goal: Point,
|
|
wind: Tuple[float, float],
|
|
obstacles: Dict[str, Polygon],
|
|
costfunction_obstacles_width: float = 40,
|
|
costfunction_obstacles_scale: float = 0.02,
|
|
) -> Optimizer:
|
|
fct = CostFunctionExp(scale=costfunction_obstacles_scale, safety_dist=costfunction_obstacles_width)
|
|
|
|
cartesian_polys: Dict[str, CartesianPolygon] = {
|
|
i: CartesianPolygon.from_shapely(k) for i, k in obstacles.items()
|
|
}
|
|
|
|
obstacle_dict: Dict[str, Obstacle] = {
|
|
i: Obstacle(poly, np.array([0, 0]), cost_function=fct) for i, poly in cartesian_polys.items()
|
|
}
|
|
|
|
context = Optimizer(wind_information=(0, 0), obstacles={}, position=Point(0, 0))
|
|
|
|
context.wind_speed, context.wind_angle = wind[0], np.deg2rad(wind[1])
|
|
|
|
context.position = position
|
|
context.goal = goal
|
|
|
|
context.on_reset_obstacles(obstacle_dict)
|
|
|
|
return context
|
|
|
|
|
|
# sueden nach norden -> wind (25 m/s, 0)
|
|
def generate_route(
|
|
position: Point,
|
|
goal: Point,
|
|
wind: Tuple[float, float],
|
|
obstacles: Dict[str, Polygon],
|
|
costfunction_obstacles_width: float = 40,
|
|
costfunction_obstacles_scale: float = 0.02,
|
|
) -> Tuple[Optional[EvaluatedTimingFrame], Optional[List[EvaluatedTimingFrame]]]:
|
|
"""Function that generates a route in the first of the tuple return value.
|
|
Second value contains a list of timing frames created during optimization.
|
|
"""
|
|
|
|
context = create_context(
|
|
position=position,
|
|
goal=goal,
|
|
wind=wind,
|
|
obstacles=obstacles,
|
|
costfunction_obstacles_width=costfunction_obstacles_width,
|
|
costfunction_obstacles_scale=costfunction_obstacles_scale,
|
|
)
|
|
|
|
return context.optimize(goal=goal, optimization_parameters=optimization_param)
|
|
|
|
|
|
# poly = Polygon([(-2, 1), (-0.1, 3), (3, 3), (2,1)])
|
|
# poly = Polygon([(-80, 10), (-50, 30), (30, 30), (20,20), (0,20)])
|
|
|
|
# poly_ = Polygon([(1000,1000),(1010,1010),(1000,1020)])
|
|
# poly2 = Polygon([(-50, 70), (50, 70), (50, 80), (-50,80)])
|
|
|
|
# ob_set = {"0": poly2, "1": poly_, "2": poly}
|
|
# ob_set_2 = {"0": poly_}
|
|
# print("START")
|
|
|
|
# route = [Point(0,0), Point(0,1), Point(0,2), Point(2,2), Point(2,1), Point(-2,2), Point(0,10)]
|
|
# _ = np.array([shapely_point_to_ndarray(p) for p in route])
|
|
# print(_)
|
|
# frame = TimingFrame(CartesianRoute.from_numpy(_))
|
|
# print(frame)
|
|
# frame2 = frame.remove_single_cycles()
|
|
|
|
# print(frame2)
|
|
# print("_"*10)
|
|
# print(frame.remove_single_cycles())
|