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, ))