def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float]:
delta_x = position[0] - self.center[0]
delta_y = position[1] - self.center[1]
phi = math.atan2(delta_y, delta_x)
phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
r = norm(delta_x, delta_y)
longitudinal = self.direction * (phi - self.start_phase) * self.radius
lateral = self.direction * (self.radius - r)
return longitudinal, lateral
def steering_control(self, target_lane) -> float:
# heading control following a lateral distance control
ego_vehicle = self.control_object
long, lat = target_lane.local_coordinates(ego_vehicle.position)
lane_heading = target_lane.heading_at(long + 1)
v_heading = ego_vehicle.heading_theta
steering = self.heading_pid.get_result(
wrap_to_pi(lane_heading - v_heading))
steering += self.lateral_pid.get_result(-lat)
return float(steering)
def steering_features(self, target_lane_index: LaneIndex) -> np.ndarray:
"""
A collection of features used to follow a lane
:param target_lane_index: index of the lane to follow
:return: a array of features
"""
lane = self.traffic_mgr.current_map.road_network.get_lane(
target_lane_index)
lane_coords = lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU
lane_future_heading = lane.heading_at(lane_next_coords)
features = np.array([
utils.wrap_to_pi(lane_future_heading - self.heading) *
self.LENGTH / utils.not_zero(self.speed),
-lane_coords[1] * self.LENGTH / (utils.not_zero(self.speed)**2)
])
return features
def steering_control(self, target_lane_index: LaneIndex) -> float:
"""
Steer the vehicle to follow the center of an given lane.
1. Lateral position is controlled by a proportional controller yielding a lateral speed command
2. Lateral speed command is converted to a heading reference
3. Heading is controlled by a proportional controller yielding a heading rate command
4. Heading rate command is converted to a steering angle
:param target_lane_index: index of the lane to follow
:return: a steering wheel angle command [rad]
"""
target_lane = self.traffic_mgr.map.road_network.get_lane(
target_lane_index)
lane_coords = target_lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU
lane_future_heading = target_lane.heading_at(lane_next_coords)
# Lateral position control
lateral_speed_command = -self.KP_LATERAL * lane_coords[1]
# Lateral speed to heading
heading_command = math.asin(
clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1))
heading_ref = lane_future_heading + clip(heading_command, -np.pi / 4,
np.pi / 4)
# Heading control
heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref -
self.heading)
# Heading rate to steering angle
steering_angle = math.asin(
clip(
self.LENGTH / 2 / utils.not_zero(self.speed) *
heading_rate_command, -1, 1))
steering_angle = clip(steering_angle, -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
return float(steering_angle)