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 extend_spots(msg: ParkingMsg): """Extend the parking spots by the spots inside msg.""" parking_spots.extend([ Polygon( Polygon(s.frame).buffer( self.parking_spot_buffer).exterior.coords) for s in msg.spots if s.type == ParkingMsg.SPOT_FREE ])
def polygons_from_msg(msg): if msg is not None: return (Polygon( Polygon(border.points).buffer( self.area_buffer).exterior.coords) for border in msg.borders) else: return []
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 __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 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 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 get_bounding_box(self) -> Polygon: """Get a polygon around the road section. Bounding box is an approximate representation of all points within a given distance of this geometric object. """ return Polygon(self.middle_line.buffer(1.5 * Config.road_width))
def draw_crossing_lines(ctx, frame: Polygon): """Draw a crossing area for pedestrian, which is only marked by dashed lines, in the given frame. The dashed lines are perpendicular to the road. Args: frame: Frame of the crossing area. **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() dash_length = 0.04 utils.draw_line( ctx, MarkedLine( [points[0], points[3]], style=RoadSection.DASHED_LINE_MARKING, ), dash_length=dash_length, ) utils.draw_line( ctx, MarkedLine( [points[1], points[2]], style=RoadSection.DASHED_LINE_MARKING, ), dash_length=dash_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 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 _show_parking_markers(self): """Publish markers for all parking lines and spots of road.""" parking_sections = [ s for s in self.get_sections().sections if s.type == road_section_type.PARKING_AREA ] parking_counter = 0 spot_counter = 0 for sec in parking_sections: parking_srv = self.get_parking(sec.id) left_msg, right_msg = parking_srv.left_msg, parking_srv.right_msg def show_borders(borders, topic_name, ns): nonlocal parking_counter for border in borders: self._publish_point_marker( border.points, topic_name, parking_counter, self.param.colors.parking_border, ns="/simulation/groundtruth/" + ns, ) parking_counter += 1 show_borders(left_msg.borders, "parking_line", "parking_left") show_borders(right_msg.borders, "parking_line", "parking_right") spots = [] spots.extend(left_msg.spots) spots.extend(right_msg.spots) for spot in spots: # Extract all four points points = Polygon(spot.frame.points).to_geometry_msg().points # self.rviz_parking_spot_publisher.publish(poly) color = self.param.colors.parking_spot_free if spot.type == ParkingMsg.SPOT_X: points = [ points[0], points[1], points[3], points[2], points[0], points[3], points[2], points[1], ] # Create an x in the visualization color = self.param.colors.parking_spot_x elif spot.type == ParkingMsg.SPOT_OCCUPIED: color = self.param.colors.parking_spot_occupied self._publish_point_marker(points, "parking_spot", spot_counter, color) spot_counter += 1
def frame(self) -> Polygon: """Polygon: Frame of the element in global coordinates.""" return Transform(self.center, self.orientation) * 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 _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 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 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 obstacles(self) -> List[Polygon]: """All obstacles as a list of polygons.""" return [ # Buffer the obstacle that is received from proxy because of some issues # with shapely thinking that the points are not a valid polygon! # And it further simplifies the polygons Polygon(o.buffer(BUFFER).exterior.coords) for sec in self.sections for o in self.get_obstacles_in_section(sec.id) ]
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 assert_line_almost_eq(line1, line2): self.assertAlmostEqual(line1.get_points()[0], line2.get_points()[0]) self.assertAlmostEqual(line1.get_points()[-1], line2.get_points()[-1]) self.assertAlmostEqual( Polygon(line1, line2).area / line1.length / line2.length, 0, delta=1e-3 ) # The polygon should have almost no area if the lines are approx. equal
def draw_parking_spot_x(ctx, frame: Polygon): points = frame.get_points() utils.draw_line( ctx, MarkedLine([points[0], points[2]], style=RoadSection.SOLID_LINE_MARKING)) utils.draw_line( ctx, MarkedLine([points[1], points[3]], style=RoadSection.SOLID_LINE_MARKING))
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
def blocked_areas(self) -> List[Polygon]: """All blocked_areas as a list of polygons.""" return [ # Buffer the blocked_area that is received from proxy because of some issues # with shapely thinking that the points are not a valid polygon! # And it further simplifies the polygons Polygon(sm.frame.buffer(BUFFER).exterior.coords) for sec in self.sections for sm in self.get_surface_markings_in_section(sec.id) if sm.id_ == SurfaceMarking.BLOCKED_AREA[0] ]
def test_speaker_properties(self): """Test properties and speak function of event speaker.""" self.assertTrue( utils.polygon_list_almost_equal( [self.speaker.right_corridor], [Polygon(self.speaker.middle_line, self.speaker.right_line)], )) self.assertTrue( utils.polygon_list_almost_equal( [self.speaker.left_corridor], [Polygon(self.speaker.left_line, self.speaker.middle_line)], )) self.assertTrue( utils.polygon_list_almost_equal( self.speaker.parking_lots, [ Polygon(self.right_border.get_points()), Polygon(self.left_border.get_points()), ], ))
def get_obstacles_in_section(self, section_id: int) -> List[Polygon]: """Get all obstacles inside as polygons. Args: section_id: id of the section Returns: List of polygons describing the frames of the obstacles """ obstacle_msgs = self.get_obstacles(section_id).polygons return [Polygon(o.frame) for o in obstacle_msgs]
def get_bounding_box(self) -> Polygon: """Get a polygon around the road section. Bounding box is an approximate representation of all points within a given distance of this geometric object. """ biggest_depth = 0 for ll, rl in zip(self.left_lots, self.right_lots): if ll.depth > biggest_depth: biggest_depth = ll.depth if rl.depth > biggest_depth: biggest_depth = rl.depth return Polygon( self.middle_line.buffer(1.5 * (biggest_depth + Config.road_width)))
def _show_obstacle_markers(self): """Publish polygons for all obstacles on the road.""" obstacles = sum( ([ Polygon(msg.frame).to_geometry_msg().points for msg in self.get_obstacles(sec.id).polygons if msg.type == LabeledPolygonMsg.OBSTACLE ] for sec in self.get_sections().sections), [], ) for id, obstacle in enumerate(obstacles): self._publish_point_marker(obstacle, "obstacle", id, self.param.colors.obstacle)
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 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 set_transform(self, tf: Transform): self._frame = Polygon([ [0, 0], [self.depth, 0], [self.depth, -self.width], [0, -self.width], ]) super().set_transform(tf) if self.obstacle is not None: self.obstacle.set_transform(self.transform) if self.x_surface_marking is not None: self.x_surface_marking.width = self.width self.x_surface_marking.depth = self.depth self.x_surface_marking._frame = self._frame self.x_surface_marking.set_transform(self.transform)