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