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 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_transformation( self, target_frame: str, source_frame: str, timestamp: rospy.rostime.Time, timeout: rospy.rostime.Duration = rospy.Duration(0.1), ) -> Optional[Transform]: """tf2 transform handler. Arguments: target_frame (str): Name of the target frame source_frame (str): Name of the source frame timestamp (rospy.rostime.Time): The time in the buffer timeout (rospy.rostime.Duration): Lookup timeout Returns: Returns the transformation. """ try: tf_transform = self.__tf2.lookup_transform(target_frame, source_frame, timestamp, timeout) return Transform(tf_transform.transform) except Exception as e: rospy.logerr(f"Could not lookup transform for message {e}") return
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 __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 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 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 __post_init__(self): assert ( self.__class__.TYPE is not None), "Subclass of RoadSection missing TYPE declaration!" if self.transform is None: self.transform = Transform([0, 0], 0)
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 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 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 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 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 next_obstacles(*xs): nonlocal prev_x for x in xs: obstacles.append(Transform([x, 0], 0) * o) self.inv_overtaking_intervals.append( (prev_x, xs[0] - overtaking_buffer)) self.overtaking_intervals.append( (xs[0] - overtaking_buffer, xs[-1] + width + overtaking_buffer)) prev_x = xs[-1] + width + overtaking_buffer
def receive_model_pose_cb(self, msg: geometry_msgs.msg.Pose): """Receive new model pose.""" rot = Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z) if self.param.set_twist: self.update_twist(self.latest_state_estimation, rot) if self.param.set_pose: self.update_pose() self.update_simulation_world_tf(vehicle_simulation_tf=Transform(msg))
def combine(rule, turn, closing, angle, size, tf_x, tf_y, tf_rot): # print(rule, turn, closing, angle, size, tf_x, tf_y, tf_rot) if (turn == closing or size / 2 * math.sin(angle) <= math.sqrt(2) * Config.road_width): return inter = Intersection(rule=rule, turn=turn, angle=angle, size=size) inter.set_transform(Transform(Point(tf_x, tf_y), tf_rot)) for obj in itertools.chain(inter.traffic_signs, inter.surface_markings, inter.obstacles): obj.frame
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 save_section(self): """Save the current custom section to a file.""" if len(self.section.middle_line_points) > 1: # Adjust initial position to zero. initial_pose = Pose(self.section.middle_line.interpolate_pose(0)) tf = Transform(initial_pose.position, initial_pose.orientation).inverse self.section.middle_line_points = [ tf * p for p in self.section.middle_line_points ] self.section.save_as_yaml(self.param.file_path)
def set_transform(self, obj: Union[Line, Transform]): """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. """ if type(obj) is Line: pose = obj.interpolate_pose( arc_length=self._center.x if self.normalize_x else 0 ) obj = Transform(pose, pose.get_angle()) super().set_transform(obj)
def set_transform(self, new_tf: Transform): super().set_transform(new_tf) for lot in self.left_lots: # Set transform to first spot lot.set_transform(self.transform * Transform( [ lot.start + lot.depth / math.tan(lot.opening_angle), Config.road_width, ], math.pi / 2, )) for lot in self.right_lots: # Set transform to last spot lot.set_transform(self.transform * Transform( [ lot.start + lot.length + lot.depth / math.tan(lot.opening_angle), -Config.road_width, ], -math.pi / 2, ))
def get_beginning(self) -> Tuple[Pose, float]: """Get the beginning of the section as a pose and the curvature. Returns: A tuple consisting of the first point on the middle line together with \ the direction facing away from the road section as a pose and the curvature \ at the beginning of the middle line. """ pose = Transform( [0, 0], math.pi) * self.middle_line.interpolate_pose(arc_length=0) curvature = self.middle_line.interpolate_curvature(arc_length=0) return (pose, curvature)
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_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 append(self, section: RoadSection): """Append a road section. Determine id of the section. Args: section: New road section. """ section.id = len(self.sections) if section.id == 0: section._is_start = True if section.id > 0: # Pass ending of last section as the transformation to next section ending: Tuple[Pose, float] = self.sections[-1].get_ending() section.set_transform(Transform(ending[0], ending[0].orientation)) section.prev_length = self.length self.length = self.length + section.middle_line.length self.sections.append(section)
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 receive_tf(self, tf_msg: TFMessage): """Receive msg from the tf topic and extract the world to vehicle transformation.""" # Only select world to vehicle transformations def select_tf(tf): return (tf.header.frame_id == self.param.frame.world and tf.child_frame_id == self.param.frame.vehicle) # Select first tf (default: None) # Beware: Transformation goes from child to header frame !! vehicle_world: geometry_msgs.msg.TransformStamped = next( filter(select_tf, tf_msg.transforms), None) if vehicle_world: self.vehicle_world_tf = Transform(vehicle_world.transform) rospy.logdebug( f"Received vehicle to world transform {vehicle_world}" f"(Converted to: {self.vehicle_world_tf} {self.vehicle_world_tf.rotation})" )
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 set_transform(self, new_tf: Transform): super().set_transform(new_tf) if self.depth is None: return spot_x = 0 """X-Value of left lower border point.""" spot_y = 0 # -self.depth / math.tan(self.opening_angle) """Y-Value of left lower border point.""" for spot in (reversed(self.spots) if self._side == ParkingLot.RIGHT_SIDE else self.spots): # Calculate local spot coordinate system spot.depth = self.depth spot._side = self._side spot.set_transform(self.transform * Transform([spot_x, spot_y], 0)) spot_y -= spot.width
def distribute_objects(attribute_name: str): """Distribute objects (traffic signs, obstacles, surface_markings, ...) to resulting sections.""" for obj in self.__getattribute__(attribute_name): x = self.middle_line.project(obj.center) if x < x_beginning: x_shift = 0 beginning.__getattribute__(attribute_name).append(obj) elif x_beginning < x < x_end: x_shift = -x_beginning section.__getattribute__(attribute_name).append(obj) elif x_end < x < x_buffered_end: x_shift = -x_end intermediate_section.__getattribute__( attribute_name).append(obj) else: x_shift = -x_buffered_end remaining_custom_section.__getattribute__( attribute_name).append(obj) obj._frame = Transform([x_shift, 0], 0) * obj._frame