def surface_markings(self) -> List[SurfaceMarkingRect]:
markings = []
if self.start_line:
# Create a start line.
markings.append(
SurfaceMarkingRect(
center=self.transform *
Point(self.start_line_length / 2, 0),
normalize_x=False,
depth=self.start_line_length,
width=2 * Config.road_width,
kind=SurfaceMarkingRect.START_LINE,
angle=self.transform.get_angle(),
))
for lot in itertools.chain(self.left_lots, self.right_lots):
for spot in lot.spots:
if spot.kind == ParkingSpot.BLOCKED:
c = spot.frame.centroid
markings.append(
SurfaceMarkingRect(
width=spot.width,
depth=spot._depth,
kind=SurfaceMarkingRect.PARKING_SPOT_X,
center=Point(c.x, c.y),
angle=spot.transform.get_angle(),
normalize_x=False,
))
return super().surface_markings + markings
def test_listen_func(self):
"""Test if car state msg is processed correctly.
* listen()
"""
# Dummy msg
pose = Pose(Point(1, 3), math.pi / 3)
frame = Polygon([Point(0, -0.3), Point(0, 0.3), Point(0.5, 0.3), Point(0.5, -0.3)])
linear_twist = Vector(2, 1, 3)
car_msg = gazebo_sim_msgs.CarState()
car_msg.pose = pose.to_geometry_msg()
car_msg.frame = frame.to_geometry_msg()
car_msg.twist = geometry_msgs.Twist()
car_msg.twist.linear = linear_twist.to_geometry_msg()
speaker = Speaker(
section_proxy=fake_msgs.section_msgs, lane_proxy=fake_msgs.lane_msgs
)
speaker.listen(car_msg)
# Test return values
self.assertEqual(speaker.car_frame, frame)
self.assertEqual(speaker.car_pose, pose)
self.assertEqual(speaker.car_speed, abs(linear_twist))
def _poly(self) -> Polygon:
opening_x = self.width / math.tan(self._opening_angle)
return Polygon([
Point(0, -Config.road_width),
Point(opening_x, -Config.road_width + self.width),
Point(self.length - opening_x, -Config.road_width + self.width),
Point(self.length, -Config.road_width),
])
def frame(self) -> Polygon:
"""Polygon: Frame of the parking spot in global coordinates."""
poly = Polygon([
Point(0, 0),
Point(self._depth, 0),
Point(self._depth, -self.width),
Point(0, -self.width),
])
return self.transform * poly
def frame(self) -> Polygon:
"""Polygon : Frame for the zebra crossing surface marking."""
poly = Polygon([
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(self.length, Config.road_width),
Point(self.length, -Config.road_width),
])
return self.transform * poly
def frame(self) -> Polygon:
poly = Polygon(
[
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(self.length, Config.road_width),
Point(self.length, -Config.road_width),
]
)
return self.transform * poly
class _RoadElementPoly(RoadElement):
frame: Polygon = Polygon(
[Point(0.3, -0.4),
Point(0.5, -0.4),
Point(0.5, 0),
Point(0.3, 0)])
"""Polygon: Frame of the element in global coordinates."""
def __post_init__(self):
super().__post_init__()
self._center = self.frame.centroid
def test_right_circular_arc(self):
TF = Transform([1, 1], math.pi / 2)
RADIUS = 1
ANGLE = math.pi / 2
rca = RightCircularArc(radius=RADIUS, angle=ANGLE, transform=TF)
self.assertEqual(rca.__class__.TYPE,
road_section_type.RIGHT_CIRCULAR_ARC)
self.assertAlmostEqual(rca.middle_line.get_points()[0], Point(TF))
self.assertAlmostEqual(rca.middle_line.get_points()[-1], Point(2, 2))
self.assert_line_is_arc(rca.middle_line, Point(2, 1), RADIUS, ANGLE)
def middle_line(self) -> Line:
points = []
t = 0
while t <= 1:
points.append(
Point(*_compute_cubic_bezier(t, self._p0, self._p1, self._p2,
self._p3)))
t += 0.01
t = 1
points.append(
Point(*_compute_cubic_bezier(t, self._p0, self._p1, self._p2,
self._p3)))
return self.transform * Line(points)
def test_left_circular_arc(self):
TF = Transform([1, 1], math.pi / 2)
RADIUS = 1
ANGLE = math.pi / 2
lca = LeftCircularArc(radius=RADIUS, angle=ANGLE)
lca.set_transform(TF)
self.assertEqual(lca.__class__.TYPE,
road_section_type.LEFT_CIRCULAR_ARC)
self.assertAlmostEqual(lca.middle_line.get_points()[0],
Point(TF.translation))
self.assertAlmostEqual(lca.middle_line.get_points()[-1], Point(0, 2))
self.assert_line_is_arc(lca.middle_line, Point(0, 1), RADIUS, ANGLE)
def test_cubic_bezier_curve(self):
TF = Transform([1, 1], math.pi / 2)
POINTS = [Point(5, 0), Point(2, 5), Point(5, 5)]
cb = CubicBezier(p1=POINTS[0], p2=POINTS[1], p3=POINTS[2])
cb.set_transform(TF)
self.assertEqual(cb.__class__.TYPE, road_section_type.CUBIC_BEZIER)
bezier_points = cb.middle_line.get_points()
self.assertPointAlmostEqual(bezier_points[0], TF * Point(0, 0))
self.assertPointAlmostEqual(bezier_points[-1], TF * POINTS[-1])
self.assert_equal_angle(cb.get_beginning()[0].get_angle(),
-math.pi + TF.get_angle())
self.assert_equal_angle(cb.get_ending()[0].get_angle(), TF.get_angle())
def get_ending(self) -> Tuple[Pose, float]:
if self.turn == Intersection.LEFT:
end_angle = math.pi / 2 + self._alpha
end_point = Point(self.z - self.w)
elif self.turn == Intersection.RIGHT:
end_angle = -math.pi / 2 + self._alpha
end_point = Point(self.z + self.w)
elif self.turn == Intersection.STRAIGHT:
end_angle = 0
end_point = Point(2 * self.z)
return (Pose(self.transform * end_point,
self.transform.get_angle() + end_angle), 0)
def read_car_config(self):
"""Process car parameters."""
car_specs = CarSpecs.from_yaml(self.param.car_specs_path)
camera_specs = CameraSpecs.from_yaml(self.param.camera_specs_path)
""" Car frame config """
# Car frame should have it's origin at the center of the rear axle:
# Distance to rear/front is measured in respect to the front axle!!
self.car_frame = Polygon(
[
[
-car_specs.distance_to_rear_bumper + car_specs.wheelbase,
car_specs.vehicle_width / 2,
],
[
-car_specs.distance_to_rear_bumper + car_specs.wheelbase,
-car_specs.vehicle_width / 2,
],
[
car_specs.distance_to_front_bumper + car_specs.wheelbase,
-car_specs.vehicle_width / 2,
],
[
car_specs.distance_to_front_bumper + car_specs.wheelbase,
car_specs.vehicle_width / 2,
],
]
)
""" Camera config """
# This parameter tells how far the camera can see
view_distance: float = camera_specs.clip["far"]
# field of view (opening angle of camera)
fov: float = camera_specs.horizontal_fov
if self.param.cone_points == 0:
self.view_cone = None
return
# Calculate a few points to approximate view frame
# Add points on horizon of our camera (at view_distance away from vehicle)
# /approximates a view cone
# Create geom.Polygon from points
self.view_cone = Polygon(
[Point(0, 0)]
+ [
Point(r=view_distance, phi=alpha)
for alpha in np.linspace(-fov / 2, fov / 2, self.param.cone_points)
]
)
def test_obstacles(self):
MIDDLE_LINE = Line([[0, 0], [0, 2]])
DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE
# Assume the obstacle class itself works!
# Simply test if the transforms etc. are set correctly.
obstacle = StaticObstacle(center=Point(1.5, -0.2), width=0.2, depth=1)
rs = DummyRoadSection(obstacles=[obstacle])
returned_obs = rs.obstacles[0]
self.assertEqual(returned_obs.transform,
Transform([0, 1.5], math.pi / 2))
self.assertEqual(returned_obs.center, Point(0.2, 1.5))
self.assertEqual(returned_obs.frame,
Polygon([[0.1, 1], [0.1, 2], [0.3, 2], [0.3, 1]]))
def test_zebra_crossing(self):
TF = Transform([1, 1], 0)
LENGTH = 2
zc = ZebraCrossing(length=LENGTH, transform=TF)
self.assertEqual(zc.__class__.TYPE, road_section_type.ZEBRA_CROSSING)
self.assertEqual(
zc.frame,
TF * Polygon([
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(LENGTH, Config.road_width),
Point(LENGTH, -Config.road_width),
]),
)
def middle_line(self) -> Line:
points = []
t = 0
while t <= 1:
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
t += 0.01
t = 1
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
return self.transform * Line(points)
def __init__(
self,
arc_length: float = 0.4,
y: float = -0.2,
width: float = width,
depth: float = depth,
angle: float = angle,
normalize_x: bool = True,
z: float = 0,
height: float = 0,
):
"""Initialize a retangular road element."""
for obj in arc_length, y, width, depth, angle:
assert isinstance(obj, float) or isinstance(
obj, int
), f"Should be a number but is {obj}"
self.width = width
self.depth = depth
self.angle = angle
self.height = height
super().__init__(
normalize_x=normalize_x,
_frame=Transform(Point(arc_length, y, z), self.angle)
* Polygon(
[
[-self.depth / 2, self.width / 2],
[self.depth / 2, self.width / 2],
[self.depth / 2, -self.width / 2],
[-self.depth / 2, -self.width / 2],
]
),
)
def start(self):
super().start()
self.detections_subscriber = rospy.Subscriber(
name=self.param.topics.detections,
data_class=TrafficSigns,
callback=self.save_msg,
)
self.publish_counter = 0
self.evaluation_publisher = rospy.Publisher(
self.param.topics.evaluation,
data_class=SignEvaluationMsg,
queue_size=10)
self.marker_publisher = rospy.Publisher(self.param.topics.sign_marking,
data_class=Marker,
queue_size=10)
rospy.wait_for_service(self.param.topics.traffic_sign)
sign_proxy = rospy.ServiceProxy(self.param.topics.traffic_sign,
LabeledPolygonSrv)
rospy.wait_for_service(self.param.topics.section)
sections = rospy.ServiceProxy(self.param.topics.section,
SectionSrv)().sections
self.signs = [
Sign(Point(Polygon(msg.frame).centroid), msg.desc, msg.visible)
for sec in sections for msg in sign_proxy(sec.id).polygons
]
def _publish_point_marker(
self,
point: Point,
id: int,
ns="simulation/sign_evaluation",
):
"""Publish an RVIZ marker on the publisher's topic.
Args:
point: Point to be published.
id: RVIZ marker id.
ns: RVIZ namespace of the marker.
"""
rospy.logdebug(f"display point {point}")
marker = visualization.get_marker(
frame_id="sim_world",
rgba=[255, 0, 255, 255],
id=id,
type=2,
ns=ns,
scale=0.05,
duration=1 / self.param.rate,
)
marker.pose.position = point.to_geometry_msg()
try:
self.marker_publisher.publish(marker)
except Exception as err:
rospy.logerr(err)
def save_msg(self, msg: TrafficSigns):
"""Add a message to the signs.
Args:
msg: The traffic sign massage from the detection.
"""
if msg.sub_messages:
tf = self.get_transformation(
"sim_world",
msg.header.frame_id,
msg.sub_messages[0].header.stamp,
timeout=rospy.Duration(0.1),
)
if tf is not None:
rospy.logdebug(
f"point before transformation {msg.sub_messages}")
points = [(tf * Point(m.pose.position),
self.name_conversion[m.type])
for m in msg.sub_messages]
for point, class_desc in points:
self.add_detection(point, class_desc)
self.publish_counter += 1
self._publish_point_marker(point, self.publish_counter)
else:
rospy.logerr(
f"Error: could not get a transformation, message: {msg.sub_messages}"
)
def test_speed_speaker(self):
speed_speaker = SpeedSpeaker(time=0)
car_msg = CarStateMsg()
speeds = []
# speed, (time_before, time_after), expected result
speeds.append((0, (0, 2), SpeakerMsg.SPEED_HALTED))
speeds.append((0, (0, 4), SpeakerMsg.SPEED_STOPPED))
speeds.append((0, (0, 0), SpeakerMsg.SPEED_0))
speeds.append((3, (0, 0), SpeakerMsg.SPEED_1_10))
speeds.append((13, (0, 0), SpeakerMsg.SPEED_11_20))
speeds.append((25, (0, 0), SpeakerMsg.SPEED_21_30))
speeds.append((32, (0, 0), SpeakerMsg.SPEED_31_40))
speeds.append((44, (0, 0), SpeakerMsg.SPEED_41_50))
speeds.append((55, (0, 0), SpeakerMsg.SPEED_51_60))
speeds.append((65, (0, 0), SpeakerMsg.SPEED_61_70))
speeds.append((73, (0, 0), SpeakerMsg.SPEED_71_80))
speeds.append((83, (0, 0), SpeakerMsg.SPEED_81_90))
speeds.append((99, (0, 0), SpeakerMsg.SPEED_91_))
for speed, time, expected in speeds:
speed_speaker.listen(car_msg)
speed_speaker.speak(current_time=time[0])
car_msg.twist.linear = Point(speed / 3.6 / 10, 0,
0).to_geometry_msg() # 1 m/s
speed_speaker.listen(car_msg)
response = speed_speaker.speak(current_time=time[1])
self.assertEqual(response[0].type, expected)
def _publish_point_marker(
self,
points: List[GeomPoint],
publisher_name: str,
id: int,
rgba: List[float],
ns="simulation/groundtruth",
):
"""Publish an RVIZ marker on the publisher's topic.
Args:
points: Points to be published.
publisher_name: Key of the publisher in the publisher dictionary.
id: RVIZ marker id.
rgba: List of the marker color.
ns: RVIZ namespace of the marker.
"""
marker = visualization.get_marker_for_points(
(Point(p) for p in points),
frame_id="simulation",
rgba=rgba,
id=id,
ns=ns,
duration=5 / self.param.rate, # Groundtruth is too slow otherwise!
)
self.publishers[publisher_name].publish(marker)
def create_points(
start_point=Point(0, 0),
offset_right=1,
point_dist=1,
section_count=10,
point_count=100,
direction=0,
deviation=math.pi / 8,
) -> List[LineTuple]:
"""Create a line of points and split it up into multiple sections."""
# Points that are biased in one direction
points = [start_point] + list(
Vector(r=point_dist,
phi=direction + (random.random() - 0.5) * 2 * deviation)
for _ in range(point_count))
# Prefix sum over the points to get the middle line points
middle_points = list(itertools.accumulate(points))
# Divide the middle points into multiple sections
def divide_chunks():
for i in range(0, point_count, int(point_count / section_count)):
yield middle_points[i:i + int(point_count / section_count) +
1 # noqa: 203
] # Also include first point of next section
middle_lines = (Line(ps) for ps in divide_chunks())
return list(
LineTuple(
middle_line.parallel_offset(offset_right, side="left"),
middle_line,
middle_line.parallel_offset(offset_right, side="right"),
) for middle_line in middle_lines)
def test_quad_bezier_curve(self):
TF = Transform([1, 1], math.pi / 2)
POINTS = [Point(5, 0), Point(2, 5)]
# simple quad bezier
qb = QuadBezier(p1=POINTS[0], p2=POINTS[1], transform=TF)
self.assertEqual(qb.__class__.TYPE, road_section_type.QUAD_BEZIER)
self.assertPointAlmostEqual(qb.middle_line.get_points()[0],
TF * Point(0, 0))
self.assertPointAlmostEqual(qb.middle_line.get_points()[-1],
TF * POINTS[1])
test_end_angle = (math.acos(
(Vector(-3, 5) * Vector(1, 0)) /
(abs(Vector(-3, 5)) * abs(Vector(1, 0)))) + TF.get_angle())
self.assert_equal_angle(qb.get_ending()[0].get_angle(), test_end_angle)
def ls_circle(self) -> Line:
if self.turn == Intersection.LEFT:
points_ls = []
for theta in arange_with_end(0, 0.5 * math.pi + self._alpha,
math.pi / 20):
points_ls.append(
Point(self.z - self.x + self.ls - self.ls.rotated(theta)))
return self.transform * Line(points_ls)
def rs_circle(self) -> Line:
if self.turn == Intersection.RIGHT:
points_rs = []
for theta in arange_with_end(0, -math.pi / 2 + self._alpha,
-math.pi / 20):
points_rs.append(
Point(self.z - self.x + self.rs - self.rs.rotated(theta)))
return self.transform * Line(points_rs)
class ParkingObstacle(StaticObstacle):
center: Point = Point(0.2, -0.2)
"""Center point of the obstacle."""
width: float = 0.15
"""Width of the obstacle."""
depth: float = 0.15
"""Width of the obstacle."""
normalize_x: bool = False
def read_car_config(self):
"""Process car parameters.
"""
car_specs = CarSpecs.from_file(self.param.car_specs_path)
camera_specs = CameraSpecs.from_file(self.param.camera_specs_path)
""" Car frame config """
chassis_size = Vector(
car_specs.distance_to_rear_bumper
# + car_specs.wheelbase
+ car_specs.distance_cog_front,
car_specs.vehicle_width,
)
chassis_position = Point(car_specs.center_rear_axle.x,
car_specs.center_rear_axle.y)
# get dimensions
x_span = Vector(0.5 * chassis_size.x,
0) # Vector in x direction of length = width/2
y_span = Vector(0, 0.5 * chassis_size.y)
self.car_frame = Polygon([
chassis_position + x_span + y_span, # Front right
chassis_position - x_span + y_span, # Front left
chassis_position - x_span - y_span, # Back left
chassis_position + x_span - y_span, # Back right
])
""" Camera config """
# This parameter tells how far the camera can see
view_distance: float = camera_specs.clip["far"]
# field of view (opening angle of camera)
fov: float = camera_specs.horizontal_fov
if self.param.cone_points == 0:
self.view_cone = None
return
# Calculate a few points to approximate view frame
# Add points on horizon of our camera (at view_distance away from vehicle) /approximates a view cone
# Create geom.Polygon from points
self.view_cone = Polygon([Point(0, 0)] + [
Point(r=view_distance, phi=alpha)
for alpha in np.linspace(-fov / 2, fov / 2, self.param.cone_points)
])
def test_blocked_area(self):
TF = Transform([1, 1], 0)
LENGTH = 2.5
WIDTH = 0.3
OPENING_ANGLE = math.radians(60)
ba = BlockedArea(length=LENGTH, width=WIDTH)
ba.set_transform(TF)
self.assertEqual(ba.__class__.TYPE, road_section_type.BLOCKED_AREA)
inset = WIDTH / math.tan(OPENING_ANGLE)
self.assertEqual(
ba.frame,
TF * Polygon([
Point(0, -Config.road_width),
Point(inset, -Config.road_width + WIDTH),
Point(LENGTH - inset, -Config.road_width + WIDTH),
Point(LENGTH, -Config.road_width),
]),
)
def __post_init__(self):
self.surface_markings.append(
SurfaceMarkingRect(
center=Point(self.length / 2, 0),
depth=self.length,
width=2 * Config.road_width,
kind=SurfaceMarkingRect.ZEBRA_CROSSING,
)
)
self.middle_line_marking = self.MISSING_LINE_MARKING
super().__post_init__()
