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