def __init__(self, name="groundtruth_node", log_level=rospy.INFO):
super().__init__(name=name, log_level=log_level)
# Initialize without a road
# The road is loaded in self.update()
road = road_module.Road()
self.groundtruth = Groundtruth(road=road)
self.renderer = Renderer(
road,
remove_model=self._remove_model,
spawn_model=self._spawn_model,
pause_gazebo=self._pause_gazebo,
unpause_gazebo=self._unpause_gazebo,
info_callback=self.update_groundtruth_status,
tile_size=Vector(self.param.tile_size),
tile_resolution=Vector(self.param.tile_resolution),
)
self.object_controller = ObjectController(
road,
remove_model=self._remove_model,
spawn_model=self._spawn_model,
)
self.model_names = []
self.car_state = CarStateMsg()
self._groundtruth_status = GroundtruthStatus()
self.run()
def bezier_points_mid_l_start(self) -> List[Point]:
return add_quad_bezier_points(
self.middle_start.to_numpy(),
(self.middle_start + Vector(self.p_offset, 0)).to_numpy(),
(self.middle_l_zebra_start - Vector(self.p_offset, 0)).to_numpy(),
self.middle_l_zebra_start.to_numpy(),
)
def bezier_points_mid_l_end(self) -> List[Point]:
return add_quad_bezier_points(
self.middle_l_zebra_end.to_numpy(),
(self.middle_l_zebra_end + Vector(self.p_offset, 0)).to_numpy(),
(self.middle_end - Vector(self.p_offset, 0)).to_numpy(),
self.middle_end.to_numpy(),
)
def _get_stop_line(line1: Line, line2: Line, kind) -> SurfaceMarkingRect:
"""Return a line perpendicular to both provided (assumed parallel) lines.
The returned line will be at the first point where both lines are parallel to each other
plus 2cm offset.
"""
beginning_line1 = line1.interpolate(0.02)
beginning_line2 = line2.interpolate(0.02)
# Test which line to draw the stop line at
if beginning_line1.distance(line2) < beginning_line2.distance(line1):
# End of line 1 is the starting point of the stop line
p1 = beginning_line1
p2 = line2.interpolate(line2.project(beginning_line1))
else:
# End of line 2 is the starting point
p1 = line1.interpolate(line1.project(beginning_line2))
p2 = beginning_line2
line = Line([p1, p2])
width = line.length
center = 0.5 * (Vector(line.coords[0]) + Vector(line.coords[1]))
angle = line1.interpolate_direction(arc_length=0).argument
return SurfaceMarkingRect(kind,
*center.xy,
angle=angle,
width=width,
normalize_x=False,
depth=0.04)
def spots(self) -> List[ParkingSpot]:
"""List[ParkingSpot]: All spots in the parking lot."""
spot_x = self.start + self.depth / math.tan(self.opening_angle)
"""X-Value of left upper border point."""
spot_y = self._side_sign * Config.road_width
"""Y-Value of left lower border point."""
for spot in self._spots:
angle = self.transform.get_angle()
# Calculate local spot coordinate system
# Local coordinate system is left on the right side of the road
if self._side == "right":
# Add math.pi if on right side
angle += math.pi * 3 / 2
origin = Vector(spot_x + spot.width, spot_y)
else:
angle += math.pi / 2
origin = Vector(spot_x, spot_y)
spot.transform = Transform(self.transform * origin, angle)
spot._depth = self.depth
spot._side = self._side
spot_x += spot.width
return self._spots
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 get_bounds(self) -> Tuple[int, int, int, int]:
"""Get the bounds of this box in the camera image.
Return:
x1, y1, x2, y2
"""
vehicle_points = self.vehicle_points
center_ground: Vector = 1 / len(vehicle_points) * sum(
vehicle_points, Vector(0, 0))
center_ground = Vector(center_ground.x, center_ground.y)
pixels = []
for p in vehicle_points + [center_ground]:
hom_vec = np.ones(4)
hom_vec[:3] = p.to_numpy()
pixel_vec = np.dot(BoundingBox.vehicle_pixel_matrix, hom_vec).T
pixel_vec *= 1 / float(pixel_vec[2])
pixels.append(Vector(pixel_vec[0], pixel_vec[1]))
pixel_ground_center = pixels[-1]
del pixels[-1]
pixel_poly = Polygon(pixels)
x1, y1, x2, y2 = pixel_poly.bounds
x1, x2 = round(min(x1, x2)), round(max(x1, x2))
y1, y2 = round(min(y1, y2)), round(max(y1, y2))
return x1, y1, x2, y2, round(pixel_ground_center.x), round(
pixel_ground_center.y)
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 start(self):
"""Start node."""
self.pub_tf = rospy.Publisher(
"/tf", TFMessage, queue_size=100
) # See: https://github.com/ros/geometry2/blob/melodic-devel/tf2_ros/src/tf2_ros/transform_broadcaster.py
self.state_estimation_publisher = rospy.Publisher(
self.param.topics.vehicle_simulation_link.state_estimation,
StateEstimationMsg,
queue_size=1,
)
groundtruth_topics = self.param.topics.groundtruth
rospy.wait_for_service(groundtruth_topics.section, timeout=30)
# Create groundtruth service proxies
self.section_proxy = rospy.ServiceProxy(groundtruth_topics.section, SectionSrv)
self.lane_proxy = rospy.ServiceProxy(groundtruth_topics.lane, LaneSrv)
# Read initial position from vehicle simulation link parameters
try:
initial = self.param.vehicle_simulation_link.initial_pose
if len(initial) > 3:
angle = initial[3]
del initial[3]
else:
angle = 0
pos = Vector(initial)
self.initial_tf = Transform(pos, angle)
except KeyError:
self.initial_tf = None
super().start()
def update(self):
"""Calculate and publish new car state information."""
# Update the driving state
current_time = rospy.Time.now().to_sec()
self._driving_state.distance_driven += (
current_time - self._driving_state.time
) * self.param.speed
self._driving_state.distance_driven %= self.driving_line.length
self._driving_state.time = current_time
rospy.logdebug(f"Current driving state: {self._driving_state}")
# Calculate position, speed, and yaw
pose = self.driving_line.interpolate_pose(self._driving_state.distance_driven)
speed = Vector(self.param.speed, 0) # Ignore y component of speed
# Yaw rate = curvature * speed
yaw_rate = (
self.driving_line.interpolate_curvature(self._driving_state.distance_driven)
* self.param.speed
)
# Publish up to date messages!
self.update_world_vehicle_tf(
self.initial_tf.inverse * Transform(pose, pose.get_angle())
)
self.update_state_estimation(speed, yaw_rate)
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 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 draw_traffic_island_blocked(ctx, frame: Polygon):
"""Draw a blocked area, which splits the two lanes of the traffic island, in the given
frame.
Args:
frame: Frame of the blocked area.
**Points of the frame must be given in the right order!**
first half of points: left border
second half: right border
"""
points = frame.get_points()
left = Line(points[:len(points) // 2])
v = Vector(Vector(points[-2]) - Vector(points[0]))
start = Vector(points[0])
STRIPES_ANGLE = math.radians(90 - 27)
STRIPES_GAP = 0.1
draw_blocked_stripes(ctx, v, start, left, points, STRIPES_ANGLE,
STRIPES_GAP)
def __init__(self):
self.vehicle_world_tf = Transform([0, 0], 0)
self.speed = Vector(0, 0)
self.yaw_rate = 0
self.vehicle_simulation_rotation = Transform([0, 0], 0)
super().__init__(
name="vehicle_simulation_link_node"
) # Name can be overwritten in launch file
self.run()
def draw_blocked_area(ctx, frame: Polygon):
"""Draw a blocked area in the given frame.
Args:
frame: Frame of the blocked area.
**Points of the frame must be given in the right order!**
first point : start on right line
second point: left of first point, towards middle line
third point : left of fourth point, towards middle line
fourth point: end on right line
Line between second and third point has to be parallel to middle/right line.
"""
points = frame.get_points()
left = Line([points[1], points[2], points[3]])
v = Vector(Vector(points[0]) - Vector(points[3]))
start = Vector(points[3])
STRIPES_ANGLE = math.radians(27)
STRIPES_GAP = 0.15
draw_blocked_stripes(ctx, v, start, left, points, STRIPES_ANGLE,
STRIPES_GAP)
def _extract_bounding_boxes(self, func):
lp_gen = (lp for sec in self._get_visible_sections()
for lp in func(sec.id))
bbs = []
for lp in lp_gen:
if lp.frame.intersects(self.camera_fov):
points = lp.frame.get_points()
points += [p + Vector(0, 0, lp.height) for p in points]
bbs.append(
BoundingBox(world_points=points,
class_id=lp.id_,
class_description=lp.desc))
return bbs
def draw(ctx, surface_marking: SurfaceMarking):
ctx.save()
if (surface_marking.kind == SurfaceMarking.LEFT_TURN_MARKING
or surface_marking.kind == SurfaceMarking.RIGHT_TURN_MARKING):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
image_file = os.path.join(
os.environ.get("KITCAR_REPO_PATH"),
"kitcar-gazebo-simulation",
"simulation",
"models",
"meshes",
f"Fahrbahnmarkierung_Pfeil_" + (
"L" if surface_marking.kind !=
SurfaceMarking.LEFT_TURN_MARKING # Weird
else "R") + ".svg",
)
svg = Rsvg.Handle().new_from_file(image_file)
ctx.scale(0.001, 0.001)
svg.render_cairo(ctx)
if (surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE
or surface_marking.kind == SurfaceMarking.STOP_LINE):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
v = 0.5 * Vector(surface_marking.width, 0)
line = MarkedLine(
[-1 * v, v],
style=(RoadSection.DASHED_LINE_MARKING
if surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE else
RoadSection.SOLID_LINE_MARKING),
)
utils.draw_line(
ctx,
line,
line_width=0.04,
dash_length=0.08,
dash_gap=0.06,
)
if surface_marking.kind == SurfaceMarking.START_LINE:
draw_start_lane(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.ZEBRA_CROSSING:
draw_zebra_crossing(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.PARKING_SPOT_X:
draw_parking_spot_x(ctx, surface_marking.frame)
ctx.restore()
class TofSensor:
name: str
origin: Origin
properties: DepthCameraProperties
size: Vector = Vector(0.02, 0.02, 0.02)
mass: float = 1e-5
@classmethod
def with_default_properties(cls, name: str, origin: Origin) -> "TofSensor":
return cls(name, origin, properties=_get_depth_properties(name))
@property
def full_name(self) -> str:
return f"ir_{self.name}"
def update_middle_line(self):
"""Update the middle line using the current position."""
current_pose = self.get_current_pose()
if current_pose is None:
return
middle_line_position = current_pose.position + Vector(0, 0.2).rotated(
current_pose.orientation).rotated(math.pi / 2)
if (len(self.section.middle_line_points) == 0
or middle_line_position.distance(
self.section.middle_line_points[-1]) >
self.param.min_point_distance):
self.section.middle_line_points.append(middle_line_position)
rospy.logdebug(f"Add new point: {middle_line_position}")
def fake_car_state(self, path: Line, arc_length: float, speed: float):
"""Publish a CarState message depending on situation.
Args:
path: The car drives on this path.
arc_length: Current arc length of the car on the path.
speed: Speed the car drives with.
"""
pose = path.interpolate_pose(arc_length=arc_length)
msg = CarStateMsg()
msg.pose = pose.to_geometry_msg()
msg.frame = (
Transform(pose.to_geometry_msg())
* Polygon([[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]])
).to_geometry_msg()
msg.twist = Twist()
msg.twist.linear = Vector(speed, 0, 0).to_geometry_msg()
self.car_state_publisher.publish(msg)
def draw_blocked_stripes(ctx, v: Vector, start: Point, line: Line,
points: List[Point], angle: float, gap: float):
"""Draw white stripes onto the ground.
White stripes are e.g. used by to signal areas on the ground
where the car is not allowed to drive.
Args:
v: Vector along the line where the stripes start points are located.
start: Start point on the line where the stripes start points are located.
line: End points of the stripes are on this line.
points: List of points of the polygon frame.
angle: Angle of the stripes.
gap: Gap between the stripes.
"""
ctx.save()
v = gap / abs(v) * v
stripe = v.rotated(math.pi + angle)
# vetcor in direction of stripes
stripe = 2.5 * Config.road_width / abs(stripe) * stripe
for point in points:
ctx.line_to(point.x, point.y)
ctx.stroke_preserve()
ctx.clip()
ctx.set_line_width(0.02)
while True:
start += v
p = Point(start + stripe)
end = line.intersection(Line([start, p]))
if end.is_empty:
break
ctx.move_to(end.x, end.y)
ctx.line_to(start.x, start.y)
ctx.stroke()
ctx.restore()
def fit_ending(_,
current_ending: Pose,
desired_ending: Pose,
control_point_distance=0.4) -> "RoadSection":
"""Add a cubic bezier curve to adjust the current ending to equal a desired ending.
Args:
current_ending: Current ending of the last section.
desired_ending: Ending that the last section should have.
control_point_distance: Distance to the bezier curve's control points.
"""
from .bezier_curve import CubicBezier # Import here to prevent cyclic dependency!
current_tf = Transform(current_ending.position,
current_ending.orientation)
d_pose = current_tf.inverse * desired_ending
cb = CubicBezier(
p1=Point(control_point_distance, 0),
p2=d_pose.position -
Vector(control_point_distance, 0).rotated(d_pose.orientation),
p3=d_pose.position,
)
return cb
def on_mouse_click(self, msg: PointMsg):
"""Calculate and publish the coordinates inside the world coordinate-system when
parsed the coordinates of a pixel on the camera image.
Arguments.
msg: Message with information about the pixel position on the camera image.
"""
# Coordinates mouse click
point_image = np.array([msg.x, msg.y, 1]).T
# Coordinates vehicle (back axis)
h_inv = inv(self.__camera_specs.H)
point_car_ = np.array(h_inv @ point_image).reshape(3)
# Normalize array
point_car = point_car_ / point_car_[2]
# Convert to sim vector
sim_vec = Vector(point_car[:2])
# Coordinates sim world
world = self.get_transformation("odom", "vehicle",
rospy.Time.now()) * sim_vec
# Publish world coordinates
self.world_publisher.publish(world.to_geometry_msg())
def __init__(
self,
name: str,
origin: Origin,
size: Vector,
mass: float,
chassis_link: Link,
):
box = Geometry(Box(size))
material = Material("mat", color=Vector(1, 1, 1))
self.link = Link(
name + "_link",
collision=Collision(geometry=box),
visual=Visual(geometry=box, material=material),
)
self.link.use_inertial_from_collision(mass)
self.joint = Joint(
name=name + "_joint",
parent=Parent(chassis_link.name),
child=Child(self.link.name),
origin=origin,
)
def angle(self):
"""float: Minimum angle between the car's direction and the object."""
e_1 = Vector(1, 0)
return math.acos(min(1 / abs(p) * p * e_1
for p in self.vehicle_points))
def vehicle_points(self) -> List[Vector]:
return list(BoundingBox.world_vehicle_tf * Vector(p)
for p in self.world_points)
def orientation(self) -> float:
"""float: Orientation between car and front of the object."""
p0, _, _, p3, *_ = self.vehicle_points
dir_vec = Vector(p3) - Vector(p0)
orientation = dir_vec.cross(Vector(0, 0, 1)).argument
return orientation
def listen(self, msg: CarStateMsg):
"""Receive information about current observations and update internal values."""
# Store car frame
self.car_frame = Polygon(msg.frame)
self.car_pose = Pose(msg.pose)
self.car_speed = abs(Vector(msg.twist.linear))
def assert_line_is_arc(self, line, center, radius, angle):
self.assertAlmostEqual(line.length, radius * angle, delta=0.001)
for p in (Transform(-1 * Vector(center), 0) * line).get_points():
self.assertAlmostEqual(abs(p), radius)
def __post_init__(self):
super().__post_init__()
self.p_offset = self.curve_area_length * self.curvature
"""Offset for the bezier control points of the curve area."""
traffic_sign_start_point = Point(
self.curve_area_length - self._sign_distance, 0)
self.traffic_signs.append(
TrafficSign(
TrafficSign.PASS_RIGHT,
*traffic_sign_start_point.xy,
angle=0,
normalize_x=False,
))
traffic_sign_end_point = Point(
self.length - self.curve_area_length + self._sign_distance, 0)
self.traffic_signs.append(
TrafficSign(
TrafficSign.PASS_RIGHT,
*traffic_sign_end_point.xy,
angle=math.pi,
normalize_x=False,
visible=False,
))
if self.zebra_marking_type == self.ZEBRA:
vector = Vector(
self.right_line.interpolate(
self.right_line.project(traffic_sign_start_point)))
point = vector + Vector(0, -0.1)
self.traffic_signs.append(
TrafficSign(
TrafficSign.ZEBRA_CROSSING,
*point.xy,
angle=0,
normalize_x=False,
))
right_poly = Polygon([
self.right_zebra_start,
self.right_zebra_end,
self.middle_r_zebra_end,
self.middle_r_zebra_start,
])
left_poly = Polygon([
self.left_zebra_start,
self.left_zebra_end,
self.middle_l_zebra_end,
self.middle_l_zebra_start,
])
if self.zebra_marking_type == self.LINES:
kind = SurfaceMarkingPoly.ZEBRA_LINES
else:
kind = SurfaceMarkingPoly.ZEBRA_CROSSING
self.surface_markings.append(
SurfaceMarkingPoly(_frame=right_poly, kind=kind,
normalize_x=False))
self.surface_markings.append(
SurfaceMarkingPoly(_frame=left_poly, kind=kind, normalize_x=False))
blocked_start_poly = Polygon([
self.middle_l_zebra_start,
*reversed(self.bezier_points_mid_l_start),
self.middle_start,
*self.bezier_points_mid_r_start,
self.middle_r_zebra_start,
])
blocked_end_poly = Polygon([
self.middle_r_zebra_end,
*self.bezier_points_mid_r_end,
self.middle_end,
*reversed(self.bezier_points_mid_l_end),
self.middle_l_zebra_end,
])
self.surface_markings.append(
SurfaceMarkingPoly(
_frame=blocked_start_poly,
kind=SurfaceMarkingPoly.TRAFFIC_ISLAND_BLOCKED,
normalize_x=False,
))
self.surface_markings.append(
SurfaceMarkingPoly(
_frame=blocked_end_poly,
kind=SurfaceMarkingPoly.TRAFFIC_ISLAND_BLOCKED,
normalize_x=False,
))
