def lane_profile(self, points_per_segment=5):
points_left = []
points_right = []
n = len(self.control_points) - 1
num = n * points_per_segment
betas = np.linspace(0, n, num=num)
for beta in betas:
q = self.center_point(beta)
delta_left = np.array([0, self.width / 2])
delta_right = np.array([0, -self.width / 2])
points_left.append(SE2_apply_R2(q, delta_left))
points_right.append(SE2_apply_R2(q, delta_right))
return points_right + list(reversed(points_left))
def get_delta(beta):
q0 = self.center_point(beta)
t0, _ = geo.translation_angle_from_SE2(q0)
d = np.linalg.norm(p - t0)
d1 = np.array([0, -d])
p1 = SE2_apply_R2(q0, d1)
d2 = np.array([0, +d])
p2 = SE2_apply_R2(q0, d2)
D2 = np.linalg.norm(p2 - p)
D1 = np.linalg.norm(p1 - p)
res = np.minimum(D1, D2)
# print('%10f: q %s %f' % (beta, geo.SE2.friendly(q0), res))
return res
def draw_svg(self, drawing, g):
assert isinstance(drawing, svgwrite.Drawing)
glane = drawing.g()
glane.attribs["class"] = "lane"
cp1 = self.control_points[0]
cp2 = self.control_points[-1]
rel = relative_pose(cp1.as_SE2(), cp2.as_SE2())
_, theta = geo.translation_angle_from_SE2(rel)
delta = int(np.sign(theta))
fill = {-1: "#577094", 0: "#709457", 1: "#947057"}[delta]
points = self.lane_profile(points_per_segment=10)
p = drawing.polygon(points=points, fill=fill, fill_opacity=0.5)
glane.add(p)
center_points = self.center_line_points(points_per_segment=10)
center_points = [
geo.translation_angle_from_SE2(_)[0] for _ in center_points
]
p = drawing.polyline(
points=center_points,
stroke=fill,
fill="none",
stroke_dasharray="0.02",
stroke_width=0.01,
)
glane.add(p)
g.add(glane)
for x in self.control_points:
q = x.asmatrix2d().m
p1, _ = geo.translation_angle_from_SE2(q)
delta_arrow = np.array([self.width / 4, 0])
p2 = SE2_apply_R2(q, delta_arrow)
gp = drawing.g()
gp.attribs["class"] = "control-point"
l = drawing.line(
start=p1.tolist(),
end=p2.tolist(),
stroke="black",
stroke_width=self.width / 20.0,
)
gp.add(l)
c = drawing.circle(
center=p1.tolist(),
r=self.width / 8,
fill="white",
stroke="black",
stroke_width=self.width / 20.0,
)
gp.add(c)
g.add(gp)