def _lane_msg_from_lines(left: Line, middle: Line, right: Line) -> LaneMsg: """Create a lane message from three lines.""" msg = LaneMsg() msg.middle_line = middle.to_geometry_msgs() msg.left_line = left.to_geometry_msgs() msg.right_line = right.to_geometry_msgs() return msg
def parking_msgs( right_spots: List[Polygon], left_spots: List[Polygon], right_border: Line, left_border: Line, id: int, ) -> groundtruth_srvs.ParkingSrvResponse: response = groundtruth_srvs.ParkingSrvResponse() right_msg = groundtruth_msgs.Parking() right_msg.borders = [ groundtruth_msgs.Line(right_border.to_geometry_msgs()) ] right_msg.spots = list( groundtruth_msgs.LabeledPolygon(spot.to_geometry_msg(), groundtruth_msgs.Parking.SPOT_FREE) for spot in right_spots) response.right_msg = right_msg left_msg = groundtruth_msgs.Parking() left_msg.borders = [groundtruth_msgs.Line(left_border.to_geometry_msgs())] left_msg.spots = list( groundtruth_msgs.LabeledPolygon(spot.to_geometry_msg(), groundtruth_msgs.Parking.SPOT_FREE) for spot in left_spots) response.left_msg = left_msg return response
def intersection_proxy(id): lane = groundtruth_msgs.Lane() if id == 2: lane.middle_line = Line([[19, 0], [20, 0]]).to_geometry_msgs() if id == 3: lane.middle_line = Line([[29, 0], [30, 0]]).to_geometry_msgs() return fake_msgs.intersection_msg(rule=self._rules[id], south=lane)
def setUp(self): n_section = 1 n_points = 100 lane_width = 0.4 # Create points lines = fake_msgs.create_points( section_count=n_section, point_count=n_points, offset_right=lane_width, direction=0, deviation=0, ) self.right_border = Line([[0, -lane_width], [1, -2 * lane_width], [2, -2 * lane_width], [3, -lane_width]]) self.left_border = Line([[0, lane_width], [1, 2 * lane_width], [2, 2 * lane_width], [3, lane_width]]) # Fake section and lane msg proxies / usually in the groundtruth package section_msg_proxy = functools.partial(fake_msgs.section_msgs, section_count=n_section) lane_msg_proxy = functools.partial(fake_msgs.lane_msgs, lines) parking_msg_proxy = functools.partial(fake_msgs.parking_msgs, [], [], self.right_border, self.left_border) self.speaker = AreaSpeaker( section_proxy=section_msg_proxy, lane_proxy=lane_msg_proxy, parking_proxy=parking_msg_proxy, min_wheel_count=3, area_buffer=0.001, )
def get_road_lines(self, section_id: int) -> LineTuple: """Request and return the road lines of a single section. Args: section_id (int): id of the section Returns: LineTuple of the section as a named tuple. """ msg = self.get_lanes(section_id).lane_msg return LineTuple(Line(msg.left_line), Line(msg.middle_line), Line(msg.right_line))
def create_intersection(id: int, rule: int = IntersectionSrvResponse.EQUAL, lane_width: float = 0.4): """Mock intersection to be used for testing. Args: id: ID of the section. rule: Priority rule of intersection. lane_width: width of right and left lane. """ tf = Transform([2 * id, 0], 0) middle_line = tf * Line([[0, 0], [2, 0]]) left_line = middle_line.parallel_offset(lane_width, side="left") right_line = middle_line.parallel_offset(lane_width, side="right") south_middle = tf * Line([[0, 0], [0.75, 0]]) north_middle = tf * Line([[1.25, 0], [2, 0]]) return section_mocks.mock_intersection( id=1, type_=road_section_type.INTERSECTION, left_line=left_line, middle_line=middle_line, right_line=right_line, turn=IntersectionSrvResponse.STRAIGHT, rule=rule, south=LineTuple( south_middle.parallel_offset(lane_width, side="left"), south_middle, south_middle.parallel_offset(lane_width, side="right"), ), west=LineTuple( tf * Line([[1 - lane_width, lane_width], [1 - lane_width, 3 * lane_width]]), tf * Line([[1, lane_width], [1, 3 * lane_width]]), tf * Line([[1 + lane_width, lane_width], [1 + lane_width, 3 * lane_width]]), ), east=LineTuple( tf * Line([[1 - lane_width, -lane_width], [1 - lane_width, -3 * lane_width]]), tf * Line([[1, -lane_width], [1, -3 * lane_width]]), tf * Line([[1 + lane_width, -lane_width], [1 + lane_width, -3 * lane_width]]), ), north=LineTuple( north_middle.parallel_offset(lane_width, side="left"), north_middle, north_middle.parallel_offset(lane_width, side="right"), ), # surface_markings=[ # ( # tf * Polygon([[0.3, -0.1], [0.3, -0.3], [0.6, -0.3], [0.6, -0.1]]), # LabeledPolygonMsg.RIGHT_TURN_MARKING, # ) # ], )
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 lines(self) -> List[MarkedLine]: """List[MarkedLine]: Borderlines for spot if spot is on the left. Marking type is always solid.""" lines = [] if self._side == ParkingLot.LEFT_SIDE or self.kind == self.BLOCKED: spot_points = self.frame.get_points() left_border = Line(spot_points[:2]) right_border = Line(spot_points[2:4]) lines.append( MarkedLine.from_line(left_border, RoadSection.SOLID_LINE_MARKING)) lines.append( MarkedLine.from_line(right_border, RoadSection.SOLID_LINE_MARKING)) return lines
def set_transform(self, line: Line): """Calculate the correct transform to this element. Depending on :attr:`self.normalize_x` the positional behavior is different. If :attr:`self.normalize_x` is True, the element is aligned along the provided line. Example: >>> from simulation.utils.geometry import Line, Point, Transform >>> from simulation.utils.road.sections.road_element import RoadElementRect >>> line = Line([Point(0, 0), Point(0, 10)]) # y axis >>> normalized_el = RoadElementRect(center=Point(1, 1)) # normalize_x is True by default >>> normalized_el.set_transform(line) >>> normalized_el.transform Transform(translation=(0.0, 1.0, 0.0),rotation=90.0 degrees) >>> normalized_el._center Point(1.0, 1.0, 0.0) >>> normalized_el.center Point(-1.0, 1.0, 0.0) >>> unnormalized_el = RoadElementRect(center=Point(1,0), normalize_x=False) ... # normalize_x is True by default >>> unnormalized_el.set_transform(line) >>> unnormalized_el.transform Transform(translation=(0.0, 0.0, 0.0),rotation=90.0 degrees) >>> normalized_el._center Point(1.0, 1.0, 0.0) >>> normalized_el.center Point(-1.0, 1.0, 0.0) """ pose = line.interpolate_pose( arc_length=self._center.x if self.normalize_x else 0) self.transform = Transform(pose, pose.get_angle())
def middle_line(self) -> Line: """Complete middle line.""" # Get the middle line of each seaction middle_line_pieces = (self.get_road_lines(sec.id).middle for sec in self.sections) # Sum it up (and start with empty line) return sum(middle_line_pieces, Line())
def right_line(self) -> Line: """Complete right line.""" # Get the right line of each seaction right_line_pieces = (self.get_road_lines(sec.id).right for sec in self.sections) # Sum it up (and start with empty line) return sum(right_line_pieces, Line())
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 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 test_straight_road(self): TF = Transform([1, 1], math.pi / 2) LENGTH = 4 MIDDLE_LINE = Line([[1, 1], [1, 5]]) sr = StraightRoad(length=LENGTH, transform=TF) self.assertEqual(sr.__class__.TYPE, road_section_type.STRAIGHT_ROAD) self.assertEqual(sr.middle_line, MIDDLE_LINE)
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)
def middle_line_l(self) -> Line: """Line: Middle line on the left side of the traffic island.""" return (self.transform * Line([ self.middle_start, *self.bezier_points_mid_l_start, self.middle_l_zebra_start, self.middle_l_zebra_end, *self.bezier_points_mid_l_end, self.middle_end, ]).simplify())
def test_road_section(self): MIDDLE_LINE = Line([[0, 0], [1, 0]]) DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE LEFT_LINE = Line([[0, Config.road_width], [1, Config.road_width]]) RIGHT_LINE = Line([[0, -Config.road_width], [1 + Config.road_width, -Config.road_width]]) ENDING = (Pose([1, 0], 0), 0) BEGINNING = (Pose([0, 0], math.pi), 0) rs = DummyRoadSection() self.assertIsNotNone(rs.transform) self.assert_lines_approx_equal(rs.middle_line, MIDDLE_LINE) self.assert_lines_approx_equal(rs.right_line, RIGHT_LINE) self.assert_lines_approx_equal(rs.left_line, LEFT_LINE) self.assertTupleEqual(rs.get_beginning(), BEGINNING) self.assertTupleEqual(rs.get_ending(), ENDING) self.assertTrue(rs.get_bounding_box().contains(LEFT_LINE)) self.assertTrue(rs.get_bounding_box().contains(MIDDLE_LINE)) self.assertTrue(rs.get_bounding_box().contains(RIGHT_LINE))
def middle_line_r(self) -> Line: """Line: Middle line on the right side of the traffic island.""" return (self.transform * Line([ self.middle_start, *self.bezier_points_mid_r_start, self.middle_r_zebra_start, self.middle_r_zebra_end, *self.bezier_points_mid_r_end, self.middle_end, ]).simplify()) """The simplification is necessary to be able to draw the blocked areas of the island.
def middle_line(self) -> Line: """Line: Middle line of the intersection.""" if self.turn == Intersection.LEFT: return self.middle_line_south + self.ls_circle + self.middle_line_west elif self.turn == Intersection.RIGHT: return self.middle_line_south + self.rs_circle + self.middle_line_east else: straight_m_l = Line([ self.middle_line_south.get_points()[-1], self.middle_line_north.get_points()[0], ]) return self.middle_line_south + straight_m_l + self.middle_line_north
def draw_start_lane(ctx, frame: Polygon): """Draw the checkerboard pattern to mark the beginning of a parking area in the given frame. Args: frame: Frame of the start lane. **Points of the frame must be given in the right order!** first point : start on left line second point: end on left line third point : end on right line fourth point: start on right line """ TILE_LENGTH = 0.02 points = frame.get_points() left = Line([points[0], points[1]]) right = Line([points[3], points[2]]) for i in range(3): utils.draw_line( ctx, MarkedLine( [ left.interpolate(TILE_LENGTH * (i + 0.5)), right.interpolate(TILE_LENGTH * (i + 0.5)), ], style=RoadSection.DASHED_LINE_MARKING, prev_length=(i % 2 + 0.5) * TILE_LENGTH, ), dash_length=TILE_LENGTH, )
def draw_zebra_crossing(ctx, frame: Polygon, stripe_width: float = 0.04, offset: float = 0.02): """Draw a zebra crossing in the given frame. Args: frame: Frame of the zebra crossing. **Points of the frame must be given in the right order!** first point : start on left line second point: end on left line third point : end on right line fourth point: start on right line """ points = frame.get_points() left = Line([points[0], points[3]]) right = Line([points[1], points[2]]) flag = True min_length = min(left.length, right.length) x = offset while x < min_length: l, r = left.interpolate(x), right.interpolate(x) if flag: ctx.move_to(l.x, l.y) ctx.line_to(r.x, r.y) flag = False else: ctx.line_to(r.x, r.y) ctx.line_to(l.x, l.y) ctx.close_path() ctx.fill() flag = True x += stripe_width
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 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 middle_line(self) -> Line: """Line: Line in the middle of the road.""" # Get all sections sections: List[int] = self.section_proxy().sections assert len(sections) > 0, ( "There must be atleast one road section. " "(The groundtruth node might not be working correctly.)" ) # Concatenate the middle line of all sections return sum( (Line(self.lane_proxy(sec.id).lane_msg.middle_line) for sec in sections), Line() )
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 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 border(self) -> Line: """Line: Outside border of the parking lot.""" if self.depth is None: return Line() border_points = [ Point(0, self.depth / math.tan(self.opening_angle)), Point( self.depth, 0, ), Point( self.depth, -self.length, ), Point( 0, -self.depth / math.tan(self.opening_angle) - self.length, ), ] border = Line( reversed(border_points) if self._side == ParkingLot.RIGHT_SIDE else border_points) return self.transform * border
def middle_line(self) -> Line: RADIAN_STEP = math.pi / 360 points = [] current_angle = 0 radius = (-1 if self.__class__.TYPE == road_section_type.RIGHT_CIRCULAR_ARC else 1) * self.radius while current_angle <= self.angle + RADIAN_STEP / 2: points.append( Point( math.cos(current_angle - math.pi / 2) * abs(radius), radius + math.sin(current_angle - math.pi / 2) * radius, )) current_angle += RADIAN_STEP return self.transform * Line(points)
def driving_line(self) -> Line: """Line: Line in the middle of the right lane (where car drives).""" # Get all sections sections: List[int] = self.section_proxy().sections assert len(sections) > 0, ( "There must be atleast one road section. " "(The groundtruth node might not be working correctly.)" ) # Concatenate the middle line of all sections middle_line = sum( (Line(self.lane_proxy(sec.id).lane_msg.middle_line) for sec in sections), Line() ) # Shift to the right to get the middle of the right lane driving_line = middle_line.parallel_offset(self.param.road_width / 2, "right") return driving_line
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)