583 lines
23 KiB
Python
583 lines
23 KiB
Python
"""Contains timing frames."""
|
|
|
|
import itertools
|
|
|
|
# Static Typing
|
|
from copy import copy
|
|
from typing import cast
|
|
from typing import Optional
|
|
from typing import Tuple
|
|
from typing import Union
|
|
|
|
import numpy.typing as npt
|
|
|
|
# Scientific Computing
|
|
import numpy as np
|
|
|
|
from scipy import linalg
|
|
|
|
import shapely.ops
|
|
from shapely.coords import CoordinateSequence
|
|
from shapely.geometry import LineString, Point
|
|
|
|
# Geometry
|
|
from pyrate.plan.geometry.location import CartesianLocation
|
|
from pyrate.plan.geometry.route import CartesianRoute
|
|
|
|
from . import utils
|
|
|
|
from .polar_model import PolarModel
|
|
|
|
|
|
class TimingFrame:
|
|
"""A wrapper class around CartesianRoutes under corresponding time and speed constraints.
|
|
|
|
Args:
|
|
route: The :class:`~pyrate.plan.geometry.route.CartesianRoute` to be wrapped
|
|
start_time: Optional clock time in seconds the frame should start at
|
|
"""
|
|
|
|
def __init__(self, route: CartesianRoute, start_time: float = 0):
|
|
self.route = route
|
|
|
|
self.identifier: int = 0
|
|
self.simulated = False
|
|
|
|
self._respect_manoeuvre = True
|
|
|
|
self._start_time = start_time
|
|
|
|
self._delta_times: npt.NDArray[np.floating] = np.array([0])
|
|
self._scalar_speeds: npt.NDArray[np.floating] = np.array([0])
|
|
self._times: npt.NDArray[np.floating] = np.arange(0, route.to_numpy().shape[0] - 1, dtype=np.float64)
|
|
self._times_without_manoeuvre: npt.NDArray[np.floating] = self.times
|
|
|
|
self._model: Optional[PolarModel] = None
|
|
self._angles: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
self._distances: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
self._end_times: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
self._directions: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
self._start_times: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
self._delta_positions: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
|
|
self._segment_points: npt.NDArray[np.floating] = utils.merge_numpy_array(
|
|
self.route.to_numpy(), self.route.to_numpy()[:, :]
|
|
)
|
|
|
|
self._speeds: npt.NDArray[np.floating] = np.array([], dtype=np.float64)
|
|
if not self._speeds.shape[0] == self.route.to_numpy().shape[0]:
|
|
temp: npt.NDArray[np.floating] = route.to_numpy()
|
|
self._speeds = cast(
|
|
npt.NDArray[np.floating], np.append((temp[1:, :] - temp[:-1, :]), [[0, 0]], axis=0)
|
|
)
|
|
|
|
def __str__(self) -> str:
|
|
return f"TimingFrame({self.route.to_numpy()})"
|
|
|
|
def __call__(self, time_to_fetch: float) -> npt.NDArray[np.floating]:
|
|
"""Give the position of the sailboat following this route at a given time
|
|
|
|
Args:
|
|
time_to_fetch: point in time for query
|
|
|
|
Returns:
|
|
Numpy array with cartesian coordinates (dim 2)
|
|
"""
|
|
time = cast(np.floating, np.float32(time_to_fetch))
|
|
|
|
if not self.simulated:
|
|
assert time >= self._times[0]
|
|
assert time <= self._times[-1]
|
|
|
|
index: int = int(np.argmax(self._times >= time) - 1)
|
|
partition = (time - self._times[index]) / (self._times[index + 1] - self._times[index])
|
|
if index == 0:
|
|
before = utils.shapely_point_to_ndarray(self.position)
|
|
else:
|
|
before = cast(npt.NDArray[np.floating], self.route.to_numpy()[index - 1])
|
|
|
|
return cast(
|
|
npt.NDArray[np.floating],
|
|
before + partition * (cast(npt.NDArray[np.floating], self.route.to_numpy()[index]) - before),
|
|
)
|
|
|
|
assert self.simulated
|
|
|
|
assert self._angles is not None
|
|
assert self._delta_positions is not None
|
|
assert self._distances is not None
|
|
assert self._directions is not None
|
|
assert self._end_times is not None
|
|
assert self._start_times is not None
|
|
|
|
assert time >= self._start_times[0]
|
|
assert time <= self._end_times[-1]
|
|
|
|
index = int(np.argmax(self._start_times >= time) - 1)
|
|
if not (self._start_times >= time).any():
|
|
index = self._start_times.shape[0] - 1
|
|
with np.errstate(divide="ignore", invalid="ignore"):
|
|
partition = (time - self._start_times[index]) / (
|
|
self._end_times[index] - self._start_times[index]
|
|
)
|
|
partition = 1.0 if partition > 1.0 else partition
|
|
if index == 0:
|
|
before = utils.shapely_point_to_ndarray(self.position)
|
|
else:
|
|
before = cast(npt.NDArray[np.floating], self.route.to_numpy()[index - 1])
|
|
return cast(
|
|
npt.NDArray[np.floating],
|
|
before + partition * (cast(npt.NDArray[np.floating], self.route.to_numpy()[index]) - before),
|
|
)
|
|
|
|
@property
|
|
def points(self) -> npt.NDArray[np.floating]:
|
|
"""Points of the route in a numpy array."""
|
|
return cast(npt.NDArray[np.floating], self.route.to_numpy())
|
|
|
|
@property
|
|
def position(self) -> CartesianLocation:
|
|
"""first point of the route"""
|
|
return CartesianLocation(east=self.points[0][0], north=self.points[0][1])
|
|
|
|
@position.setter
|
|
def position(self, value: CartesianLocation) -> None:
|
|
tmp = self.points
|
|
|
|
tmp[0][0] = value.east
|
|
tmp[0][1] = value.north
|
|
|
|
self.route = CartesianRoute.from_numpy(tmp)
|
|
|
|
@property
|
|
def start_time(self) -> float:
|
|
"""Start time of the :class:`TimingFrame`."""
|
|
return self._start_time
|
|
|
|
@property
|
|
def speeds(self) -> npt.NDArray[np.floating]:
|
|
"""Speeds of the route segments in a numpy array."""
|
|
return self._speeds
|
|
|
|
@property
|
|
def times(self) -> npt.NDArray[np.floating]:
|
|
"""Returns the timing of the route segments.
|
|
|
|
After :meth:`update_times` has been called, this includes manoeuvre times.
|
|
"""
|
|
return self._times
|
|
|
|
@property
|
|
def segment_points(self) -> npt.NDArray[np.floating]:
|
|
"""The points of each segment as a numpy array representation."""
|
|
return self._segment_points
|
|
|
|
@property
|
|
def valid(self) -> bool:
|
|
"""``True`` iff it was already evaluated and no collision has been found."""
|
|
return False # Overridden in superclass
|
|
|
|
@property
|
|
def cost(self) -> float:
|
|
"""Cost of the route. This is only describes the time it needs to take the route.
|
|
|
|
Warning:
|
|
Only really accurate after :meth:`update_times` has been called
|
|
"""
|
|
return float(self._times[-1])
|
|
|
|
def update_times(self, boat_model: Optional[PolarModel] = None) -> float:
|
|
"""Updates the timing constraint of a :class:`TimingFrame` to adapt to a polar model.
|
|
|
|
This fills in manoeuvre times, angles and speed information.
|
|
|
|
Args:
|
|
boat_model: boat model to adapt to
|
|
|
|
Returns:
|
|
time needed to reach goal under given constraints
|
|
"""
|
|
if boat_model is not None:
|
|
self._model = boat_model
|
|
else:
|
|
assert self._model
|
|
|
|
self._delta_positions = cast(
|
|
npt.NDArray[np.floating],
|
|
self.route.to_numpy()[1:] - self.route.to_numpy()[:-1, :],
|
|
)
|
|
|
|
self._distances = cast(npt.NDArray[np.floating], linalg.norm(self._delta_positions, axis=1))
|
|
|
|
with np.errstate(divide="ignore", invalid="ignore"):
|
|
self._directions = cast(
|
|
npt.NDArray[np.floating], self._delta_positions / self._distances[:, None]
|
|
)
|
|
self._directions[self._distances == 0, :] = 0
|
|
|
|
self._angles = cast(
|
|
npt.NDArray[np.floating], np.arctan2(self._delta_positions[:, 1], self._delta_positions[:, 0])
|
|
)
|
|
|
|
# apply transformation to right hand y axis
|
|
self._angles = np.vectorize(utils.transform_angles_leftx_to_righty, signature="()->()")(self._angles)
|
|
|
|
self._scalar_speeds = cast(
|
|
npt.NDArray[np.floating], np.clip(self._model.speed(self._angles), 1e-3, None).astype(np.float64)
|
|
)
|
|
|
|
self._delta_times = cast(
|
|
npt.NDArray[np.floating], np.clip(self._distances / self._scalar_speeds, 0, None)
|
|
)
|
|
|
|
self._speeds = cast(npt.NDArray[np.floating], self._scalar_speeds[:, None] * self._directions)
|
|
|
|
self._speeds[self._directions == 0] = 0
|
|
|
|
self._times = np.concatenate((np.zeros(1), np.cumsum(self._delta_times)))
|
|
self._times += self._start_time
|
|
|
|
self._times_without_manoeuvre = self._times
|
|
|
|
self._add_manoeuvre_time(self._angles)
|
|
|
|
self.simulated = True
|
|
|
|
return float(self.times[-1])
|
|
|
|
def prune(self, eps: float = 0.01) -> None:
|
|
"""Function to delete subgoals with neglectable direction changes, to reduce route points.
|
|
|
|
Args:
|
|
eps: angle difference threshold for deciding on the edge deletion (in radians)
|
|
"""
|
|
delta_pos = self.points - np.concatenate(
|
|
(utils.shapely_point_to_ndarray(self.position)[None, :], self.points[:-1, :]), axis=0
|
|
)
|
|
angles = np.arctan2(delta_pos[:, 1], delta_pos[:, 0])
|
|
# calculate angle differences
|
|
delta_ang = np.abs(angles[1:] - angles[:-1])
|
|
# create a mask from angle differences between each segment and given threshold
|
|
mask = np.append(delta_ang > eps, True)
|
|
mask[0] = True
|
|
mask[-1] = True
|
|
|
|
self.route = CartesianRoute.from_numpy(self.route.to_numpy()[mask])
|
|
|
|
def cost_grad(
|
|
self,
|
|
other_cost_dtimes: Optional[npt.NDArray[np.floating]] = None,
|
|
dcost_dpoints_ext: Optional[npt.NDArray[np.floating]] = None,
|
|
dcost_dspeed: Optional[npt.NDArray[np.floating]] = None,
|
|
time_cost: float = 2,
|
|
) -> npt.NDArray[np.floating]:
|
|
"""Calculates the cost gradient(derivative) of a ``TimingFrame`` with to subgoal timings.
|
|
|
|
If other time gradients are given returns gradient with respect to all variables.
|
|
|
|
Args:
|
|
other_cost_dtimes: calculated gradient w.r.t timings of ``(number of segments, 2)``
|
|
dcost_dpoints_ext: calculated gradient w.r.t location of obstacles of ``(number of segments, 2)``
|
|
dcost_dspeed: calculated gradient in respect to speed of obstacles of ``(number of segments, 2)``
|
|
time_cost: coefficient for cost per timing interval
|
|
|
|
Returns: cost gradient of the ``TimingFrame`` cumulated w.r.t. the subgoal timings, segment speeds,
|
|
segment times and optionally additionally given obstacle gradients. This gradient is of the shape
|
|
``(number of segments, 2)``
|
|
"""
|
|
|
|
grad = np.zeros(self._times.shape[0] - 1)
|
|
|
|
grad[-1] = np.float64(time_cost)
|
|
|
|
# handle case if no partial gradients are given
|
|
if other_cost_dtimes is None or dcost_dpoints_ext is None or dcost_dspeed is None:
|
|
return self._gradients(grad)
|
|
|
|
if self._respect_manoeuvre:
|
|
dcost_dpoints = dcost_dpoints_ext[:-1:2] + dcost_dpoints_ext[1::2]
|
|
|
|
else:
|
|
dcost_dpoints = dcost_dpoints_ext
|
|
|
|
# assure mypy and us that there are no overflows
|
|
assert not np.isnan(grad + other_cost_dtimes[:-1]).any(), (grad, other_cost_dtimes[:-1])
|
|
|
|
return self._gradients(grad + other_cost_dtimes[:-1], dcost_dspeed) + cast(
|
|
npt.NDArray[np.floating], dcost_dpoints
|
|
)
|
|
|
|
def _gradients(
|
|
self,
|
|
dcost_dtimes: npt.NDArray[np.floating],
|
|
dcost_speed: npt.NDArray[np.floating] = np.array([], dtype=np.float64),
|
|
) -> npt.NDArray[np.floating]:
|
|
"""Calculates the gradients of the time events with respect to the route edge coordinates.
|
|
|
|
Also calculates the gradient of the vector speeds for each sequence.
|
|
|
|
Args:
|
|
dcost_dtimes: previously calculated gradient in respect to timings
|
|
dcost_speed: previously calculated gradient in respect to speed of obstacles and speed
|
|
on route segments
|
|
|
|
Returns: cost gradient of the :class:`TimingFrame` for time and speed, of the shape
|
|
``(number of segments, 2)``
|
|
"""
|
|
|
|
if dcost_speed.size == 0:
|
|
dcost_speed = np.zeros((dcost_dtimes.shape[0], 2))
|
|
|
|
cost_grad_delta_times = np.flipud(np.cumsum(np.flipud(dcost_dtimes)))
|
|
|
|
if not self._respect_manoeuvre:
|
|
dcost_dtimes_basic = cost_grad_delta_times
|
|
else:
|
|
dcost_dtimes_basic = cost_grad_delta_times[::2]
|
|
dcost_speed = dcost_speed[::2, :]
|
|
|
|
assert not np.isnan(dcost_dtimes_basic).any(), dcost_dtimes
|
|
|
|
dcost_ddist = dcost_dtimes_basic / self._scalar_speeds
|
|
dcost_dscal_speed = -dcost_dtimes_basic * self._distances / self._scalar_speeds ** 2
|
|
dcost_dscal_speed += np.einsum("ij, ij-> i", dcost_speed, self._directions)
|
|
|
|
dcost_dscal_speed = np.nan_to_num(dcost_dscal_speed)
|
|
|
|
dcost_ddirections = dcost_speed * self._scalar_speeds[:, None]
|
|
|
|
assert self._model is not None
|
|
# print("*"*30)
|
|
# print(dcost_speed)
|
|
# print(dcost_dscal_speed)
|
|
# print(self._scalar_speeds)
|
|
# print(self._directions)
|
|
# print("_"*30)
|
|
# print(dcost_ddist)
|
|
# print("-*-"*10)
|
|
# print(dcost_dscal_speed)
|
|
# print("/"*30)
|
|
# print(self._model.speed_grad(self._angles))
|
|
dcost_dang = dcost_dscal_speed * self._model.speed_grad(self._angles)
|
|
# print("-o-"*10)
|
|
# print(dcost_dang)
|
|
if self._respect_manoeuvre:
|
|
dcost_dang += self._manoeuvre_time(self._angles, cost_grad_delta_times[1::2])[1]
|
|
|
|
dcost_ddist = np.nan_to_num(dcost_ddist)
|
|
|
|
assert not np.isnan(dcost_ddist).any(), self._scalar_speeds
|
|
assert not np.isnan(dcost_dang).any(), (dcost_dscal_speed, dcost_dang)
|
|
assert not np.isnan(dcost_ddirections).any(), dcost_ddirections
|
|
|
|
return self._transpose_gradients_to_polar(dcost_ddist, dcost_dang, dcost_ddirections)
|
|
|
|
def _add_manoeuvre_time(self, angles: npt.NDArray[np.floating]) -> None:
|
|
"""Calculates and appends manoeuvre time to timing vector of the :class:`TimingFrame`
|
|
|
|
Args:
|
|
angles: angles ot the :class:`TimingFrame` of shape ``(number of segments - 1, )``
|
|
"""
|
|
|
|
delta_times_manoeuvre = self._manoeuvre_time(angles)
|
|
# print("+-+"*10)
|
|
# print(delta_times_manoeuvre)
|
|
# print("+-+"*10)
|
|
times_manoeuvre = np.cumsum(delta_times_manoeuvre)
|
|
|
|
self._start_times = self._times_without_manoeuvre[:-1] + np.concatenate(
|
|
(np.zeros(1), times_manoeuvre)
|
|
)
|
|
self._end_times = self._times_without_manoeuvre[1:] + np.concatenate((np.zeros(1), times_manoeuvre))
|
|
|
|
# print(f"{self._start_times} -<>- {self._end_times}")
|
|
self._times = utils.merge_numpy_array(self._start_times, self._end_times)
|
|
# print(f"{self._times}")
|
|
self._speeds = utils.merge_numpy_array(self._speeds, np.zeros(self._speeds.shape)[:-1, :])
|
|
self._segment_points = utils.merge_numpy_array(self.route.to_numpy()[1:], self.route.to_numpy()[1:-1])
|
|
|
|
def _manoeuvre_time(
|
|
self,
|
|
angles,
|
|
dcost_dtimes: npt.NDArray[np.floating] = np.array([], dtype=np.float64),
|
|
time_loss: Optional[np.floating] = None,
|
|
c_angle: float = 10.0,
|
|
) -> Union[npt.NDArray[np.floating], Tuple[npt.NDArray[np.floating], npt.NDArray[np.floating]]]:
|
|
"""Calculates the needed time for each maneuver at each subgoal.
|
|
|
|
If the cost gradient with respect to time is given also returns the partial cost derivative
|
|
with regards to manoeuvre simulation.
|
|
|
|
Args:
|
|
angles: array of turning angles at each subgoal of the shape ``(number of route segments, )``
|
|
dcost_dtimes: previously calculated cost derivative w.r.t time ``(number of route segments, )``
|
|
time_loss: time loss coefficient to be applied at each turning
|
|
c_angle: a coefficient for scaling the angle
|
|
|
|
Returns:
|
|
Calculated additional time needed for each turning as an array of shape ``(number of route
|
|
segments, )``. If the partial derivative w.r.t speed is given also returns an additional gradient
|
|
of shape ``(number of route segments, )`` w.r.t the angles. This gradient is simulated via an
|
|
exponential cost function.
|
|
"""
|
|
|
|
if time_loss is None:
|
|
assert self._model is not None
|
|
time_loss = self._model.manoeuvre_time
|
|
d_angle = angles[1:] - angles[:-1]
|
|
|
|
d_angle = -np.pi + ((d_angle + np.pi) % (2 * np.pi))
|
|
|
|
scale = c_angle / 180.0 * np.pi
|
|
|
|
times = time_loss * (1 - np.exp(-(d_angle ** 2) / scale ** 2))
|
|
|
|
if dcost_dtimes.shape[0] == 0:
|
|
return cast(npt.NDArray[np.floating], times)
|
|
|
|
d_dang = time_loss * np.exp(-(d_angle ** 2) / scale ** 2) / scale ** 2 * dcost_dtimes * 2 * d_angle
|
|
return times, np.append(-d_dang, 0) + np.concatenate((np.zeros(1), d_dang))
|
|
|
|
def _transpose_gradients_to_polar(
|
|
self,
|
|
dcost_ddist: npt.NDArray[np.floating],
|
|
dcost_dang: npt.NDArray[np.floating],
|
|
dcost_ddirections: npt.NDArray[np.floating],
|
|
) -> npt.NDArray[np.floating]:
|
|
"""Calculate the cumulated gradient for the cartesian to polar transform according to chain rule.
|
|
|
|
Args:
|
|
dcost_ddist: cost gradient w.r.t distance vectors of shape ``(number of route segments, 2)``
|
|
dcost_dang: cost gradient w.r.t segment angles ``(number of route segments, )``
|
|
dcost_ddirections: cost gradient w.r.t direction vectors ``(number of route segments, 2)``
|
|
|
|
Returns:
|
|
transposed and cumulated gradient of shape ``(number of route segments, 2)``
|
|
"""
|
|
|
|
with np.errstate(divide="ignore", invalid="ignore"):
|
|
addition = -(
|
|
np.einsum("ij, ij-> i", dcost_ddirections, self._delta_positions) / self._distances ** 2
|
|
)
|
|
addition = np.nan_to_num(addition)
|
|
|
|
assert not np.isnan(addition).any(), addition
|
|
|
|
dcost_ddist += addition
|
|
|
|
# print(f"dc_ddist {dcost_ddist}")
|
|
# print(f"dc_ddist {dcost_ddirections}")
|
|
# print(f"dc_dang {dcost_dang}")
|
|
|
|
# print(f"addition {addition}")
|
|
# print(f"distances {self._distances}")
|
|
|
|
# print(f"rotdelta {np.concatenate((-self._delta_positions[:, 1:2], self._delta_positions[:, 0:1]), axis=1)}")
|
|
# print(f"unclipped {(dcost_dang / self._distances**2)}")
|
|
# print(f"clipped {np.clip((dcost_dang / self._distances**2), None, 1e6)}")
|
|
# assert non NaN values
|
|
|
|
assert not np.isnan(dcost_ddirections).any(), dcost_ddirections
|
|
assert not np.isnan(dcost_ddist).any(), (dcost_ddist, self._distances)
|
|
assert not np.isnan(dcost_dang).any(), dcost_dang
|
|
|
|
grad_delta: npt.NDArray[np.floating] = (
|
|
dcost_ddirections / self._distances[:, None]
|
|
+ (dcost_ddist / self._distances)[:, None] * self._delta_positions
|
|
+ np.concatenate((-self._delta_positions[:, 1:2], self._delta_positions[:, 0:1]), axis=1)
|
|
* np.clip((dcost_dang / self._distances ** 2), None, 1e6)[:, None]
|
|
)
|
|
|
|
# print(f"graddelta {grad_delta}")
|
|
grad_delta[self._distances == 0, :] = 0.0
|
|
|
|
# remove NaN values because first manoeuvre should'nt be moved
|
|
|
|
assert not np.isnan(grad_delta).any(), grad_delta
|
|
|
|
grad = -grad_delta[1:, :] + grad_delta[:-1, :]
|
|
|
|
# print(f"grad {grad}")
|
|
assert not np.isnan(grad).any(), (grad, grad_delta)
|
|
|
|
return cast(npt.NDArray[np.floating], grad)
|
|
|
|
@staticmethod
|
|
def detect_crossing(linestring: LineString) -> Optional[CartesianLocation]:
|
|
segments = list(map(LineString, zip(linestring.coords[:-1], linestring.coords[1:])))
|
|
|
|
for seg1, seg2 in itertools.combinations(segments, 2):
|
|
if seg1.coords[0] == seg2.coords[0]:
|
|
return CartesianLocation.from_shapely(Point(seg1.coords[-0]))
|
|
# return CartesianLocation.from_shapely(Point(seg1.coords[-0])), CartesianLocation.from_shapely(Point(seg2.coords[-0]))
|
|
if seg1.crosses(seg2):
|
|
return seg1.intersection(seg2)
|
|
# return seg1.intersection(seg2), seg2.intersection(seg1)
|
|
# todo (BEN) refactor
|
|
|
|
return None
|
|
|
|
def remove_single_cycles(self) -> "TimingFrame":
|
|
|
|
simple_route = []
|
|
route_to_work = self.route
|
|
crossing = TimingFrame.detect_crossing(route_to_work)
|
|
|
|
# while there are crossings
|
|
while crossing:
|
|
line_strings_start, remaining_route = list(shapely.ops.split(route_to_work, crossing))
|
|
# keep before self intersection
|
|
simple_route.append(line_strings_start)
|
|
|
|
# remaining_route = line_strings_split[1]
|
|
# if the last point / line is the line / point being crossed there is no element after.
|
|
if len(remaining_route.coords) <= 2:
|
|
final_route_elements = []
|
|
for part in simple_route:
|
|
final_route_elements += list(part.coords)
|
|
final_route_elements += remaining_route.coords[-1:]
|
|
linestring_without_cycle = LineString(final_route_elements)
|
|
|
|
return TimingFrame(CartesianRoute.from_shapely(linestring_without_cycle))
|
|
# todo (BEN) Refactor
|
|
|
|
remaining_route = LineString(remaining_route.coords[1:][::-1]) # start from next coordinates
|
|
buffed_crossing = crossing.buffer(0.001)
|
|
line_string_end = shapely.ops.split(remaining_route, buffed_crossing)[0]
|
|
line_string_end = LineString(line_string_end.coords[::-1])
|
|
|
|
if len(line_string_end.coords) <= 2:
|
|
final_route_elements = []
|
|
for part in simple_route:
|
|
final_route_elements += list(part.coords)
|
|
final_route_elements += line_string_end.coords[-1:]
|
|
linestring_without_cycle = LineString(final_route_elements)
|
|
return TimingFrame(CartesianRoute.from_shapely(linestring_without_cycle))
|
|
|
|
line_string_end = LineString(line_string_end.coords[1:])
|
|
# repeat
|
|
|
|
crossing = TimingFrame.detect_crossing(line_string_end)
|
|
route_to_work = line_string_end
|
|
# if there are no more cycles keep the rest
|
|
simple_route.append(route_to_work)
|
|
|
|
final_route_elements = []
|
|
for part in simple_route:
|
|
final_route_elements += list(part.coords)
|
|
linestring_without_cycle = LineString(final_route_elements)
|
|
return TimingFrame(CartesianRoute.from_shapely(linestring_without_cycle))
|
|
|
|
def append(self, route: CartesianRoute) -> "TimingFrame":
|
|
_ = np.concatenate((self.route.to_numpy(), route.to_numpy()), axis=0)
|
|
return TimingFrame(CartesianRoute.from_numpy(_), start_time=self.start_time)
|
|
|
|
def prepend(self, route: CartesianRoute) -> "TimingFrame":
|
|
_ = np.concatenate((route.to_numpy(), self.route.to_numpy()), axis=0)
|
|
return TimingFrame(CartesianRoute.from_numpy(_), start_time=self.start_time)
|
|
|
|
def inject(self, ind: int, route: CartesianRoute):
|
|
_ = np.concatenate((route.to_numpy()[0 : ind - 1, 0:1], self.route.to_numpy()), axis=0)
|
|
_ = np.concatenate((_, route.to_numpy()[ind - 1 :, 0:1]), axis=0)
|
|
return TimingFrame(CartesianRoute.from_numpy(_), start_time=self.start_time)
|