def getSegmentArc(
spline: np.array,
t0: float,
t1: float,
max_dx: float,
max_dy: float,
max_dtheta: float,
) -> np.array:
p0 = spline.getPoint(t0)
p1 = spline.getPoint(t1)
r0 = spline.getHeading(t0)
r1 = spline.getHeading(t1)
transform_point = (p1 - p0).rotateBy(-r0)
transformation = Pose(transform_point.x, transform_point.y,
boundRadians(r1 - r0))
twist = transformation.getTwist()
if twist.dx > max_dx or twist.dy > max_dy or twist.dtheta > max_dtheta:
return np.concatenate((
getSegmentArc(spline, t0, (t0 + t1) / 2, max_dx, max_dy,
max_dtheta),
getSegmentArc(spline, (t0 + t1) / 2, t1, max_dx, max_dy,
max_dtheta),
))
else:
return np.array([spline.getPoseWithCurvature(t1)])
